NASA Astrobee Robot Software  Astrobee Version:
Flight software for the Astrobee robots operating inside the International Space Station.
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
utilities.h
Go to the documentation of this file.
1 /* Copyright (c) 2017, United States Government, as represented by the
2  * Administrator of the National Aeronautics and Space Administration.
3  *
4  * All rights reserved.
5  *
6  * The Astrobee platform is licensed under the Apache License, Version 2.0
7  * (the "License"); you may not use this file except in compliance with the
8  * License. You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15  * License for the specific language governing permissions and limitations
16  * under the License.
17  */
18 #ifndef POINT_CLOUD_COMMON_UTILITIES_H_
19 #define POINT_CLOUD_COMMON_UTILITIES_H_
20 
21 #include <localization_common/logger.h>
22 #include <point_cloud_common/organized_neighbor2.h>
23 #include <point_cloud_common/organized_neighbor2_impl.h>
24 
25 #include <gtsam/geometry/Pose3.h>
26 #include <gtsam/geometry/Point3.h>
27 
28 #include <pcl/features/fpfh.h>
29 #include <pcl/features/integral_image_normal.h>
30 #include <pcl/features/normal_3d.h>
31 #include <pcl/features/impl/normal_3d.hpp>
32 #include <pcl/filters/fast_bilateral.h>
33 #include <pcl/filters/impl/fast_bilateral.hpp>
34 #include <pcl/filters/normal_space.h>
35 #include <pcl/filters/voxel_grid.h>
36 #include <pcl/kdtree/impl/kdtree_flann.hpp>
37 #include <pcl/point_cloud.h>
38 #include <pcl/point_types.h>
39 #include <pcl/search/impl/kdtree.hpp>
40 #include <pcl/search/impl/search.hpp>
41 
42 #include <limits>
43 #include <vector>
44 
45 namespace point_cloud_common {
46 template <typename PointType, typename PointWithNormalType>
47 void EstimateNormals(const typename pcl::PointCloud<PointType>::Ptr cloud, const double search_radius,
48  pcl::PointCloud<PointWithNormalType>& cloud_with_normals);
49 
50 template <typename PointType, typename PointWithNormalType>
51 void EstimateOrganizedNormals(
52  const typename pcl::PointCloud<PointType>::Ptr cloud,
53  const typename pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::NormalEstimationMethod method,
54  const bool depth_dependent_smoothing, const double max_depth_change_factor, const double normal_smoothing_size,
55  pcl::PointCloud<PointWithNormalType>& cloud_with_normals);
56 
57 template <typename PointType>
58 void NormalSpaceSubsampling(typename pcl::PointCloud<PointType>::Ptr cloud, const int bins_per_axis,
59  const int num_samples);
60 
61 Eigen::Matrix4f RansacIA(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr source_cloud,
62  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr target_cloud);
63 
64 pcl::PointCloud<pcl::FPFHSignature33>::Ptr EstimateHistogramFeatures(
65  const pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_with_normals);
66 
67 Eigen::Matrix<double, 1, 6> PointToPlaneJacobian(const gtsam::Point3& source_point, const gtsam::Vector3& normal,
68  const gtsam::Pose3& target_T_source);
69 
70 Eigen::Matrix<double, 3, 6> PointToPointJacobian(const gtsam::Point3& source_point,
71  const gtsam::Pose3& target_T_source);
72 
73 Eigen::Isometry3d RelativeTransformUmeyama(const std::vector<Eigen::Vector3d>& source_points,
74  const std::vector<Eigen::Vector3d>& target_points);
75 
76 template <typename PointType>
77 boost::optional<Eigen::Vector3d> GetNormal(const Eigen::Vector3d& point, const pcl::PointCloud<PointType>& cloud,
78  const pcl::search::KdTree<PointType>& kdtree,
79  const double search_radius = 0.03);
80 
81 template <typename PointType>
82 bool computePointNormal(const pcl::PointCloud<PointType>& cloud, const std::vector<int>& indices, float& normal_x,
83  float& normal_y, float& normal_z, float& curvature);
84 
85 boost::optional<Eigen::Matrix<double, 6, 6>> PointToPointCovariance(const std::vector<Eigen::Vector3d>& source_points,
86  const Eigen::Isometry3d& target_T_source);
87 
88 boost::optional<Eigen::Matrix<double, 6, 6>> PointToPlaneCovariance(const std::vector<Eigen::Vector3d>& source_points,
89  const std::vector<Eigen::Vector3d>& target_normals,
90  const Eigen::Isometry3d& target_T_source);
91 
92 template <typename PointType>
93 Eigen::Vector3d Vector3d(const PointType& point);
94 
95 template <typename PointType>
96 Eigen::Vector3d NormalVector3d(const PointType& point_with_normal);
97 
98 pcl::PointXYZI Interpolate(const pcl::PointXYZI& point_a, const pcl::PointXYZI& point_b, const double alpha);
99 
100 template <typename PointType>
101 bool ValidPoint(const PointType& point) = delete;
102 
103 template <>
104 bool ValidPoint<pcl::PointXYZ>(const pcl::PointXYZ& point);
105 
106 template <>
107 bool ValidPoint<pcl::PointXYZI>(const pcl::PointXYZI& point);
108 
109 template <>
110 bool ValidPoint<pcl::PointNormal>(const pcl::PointNormal& point);
111 
112 template <>
113 bool ValidPoint<pcl::PointXYZINormal>(const pcl::PointXYZINormal& point);
114 
115 template <typename Type>
116 bool ApproxZero(const Type& point, const double epsilon = 1e-5) {
117  return std::abs(point) <= epsilon;
118 }
119 
120 template <typename PointXYZType>
121 bool NonzeroPointXYZ(const PointXYZType& point);
122 
123 template <typename PointXYZType>
124 bool ValidPointXYZ(const PointXYZType& point);
125 
126 template <typename PointNormalType>
127 bool ValidNormal(const PointNormalType& point);
128 
129 template <typename PointIntensityType>
130 bool ValidIntensity(const PointIntensityType& point);
131 
132 template <typename PointType>
133 void RemoveInvalidAndZeroPoints(pcl::PointCloud<PointType>& cloud);
134 
135 // Some pcl functions expect invalid points to be labeled with nans while
136 // some sensors label invalid points with all zeros.
137 template <typename PointType>
138 void ReplaceZerosWithNans(typename pcl::PointCloud<PointType>& cloud);
139 
140 template <typename PointType>
141 typename pcl::PointCloud<PointType>::Ptr DownsamplePointCloud(const typename pcl::PointCloud<PointType>::Ptr cloud,
142  const double leaf_size);
143 
144 template <typename PointType>
145 typename pcl::PointCloud<PointType>::Ptr BilateralFilterOrganizedCloud(
146  const typename pcl::PointCloud<PointType>::Ptr cloud, const double sigma_s, const double sigma_r);
147 template <typename PointType>
148 void FilterCorrespondences(const typename pcl::PointCloud<PointType>& input_cloud,
149  const typename pcl::PointCloud<PointType>& target_cloud,
150  pcl::Correspondences& correspondences);
151 
152 template <typename PointType>
153 typename pcl::PointCloud<PointType>::Ptr FilteredPointCloud(
154  const typename pcl::PointCloud<PointType>::Ptr unfiltered_cloud);
155 
156 template <typename PointType, typename PointWithNormalType>
157 typename pcl::PointCloud<PointWithNormalType>::Ptr FilteredPointCloudWithNormals(
158  const typename pcl::PointCloud<PointType>::Ptr unfiltered_cloud, const double search_radius);
159 
160 // Implementation
161 template <typename PointXYZType>
162 bool NonzeroPointXYZ(const PointXYZType& point) {
163  bool nonzero_point = !ApproxZero(point.x) || !ApproxZero(point.y) || !ApproxZero(point.z);
164  return nonzero_point;
165 }
166 
167 template <typename PointXYZType>
168 bool ValidPointXYZ(const PointXYZType& point) {
169  const bool finite_point = pcl_isfinite(point.x) && pcl_isfinite(point.y) && pcl_isfinite(point.z);
170  const bool nonzero_point = NonzeroPointXYZ(point);
171  return finite_point && nonzero_point;
172 }
173 
174 template <typename PointNormalType>
175 bool ValidNormal(const PointNormalType& point) {
176  const bool finite_normal =
177  pcl_isfinite(point.normal_x) && pcl_isfinite(point.normal_y) && pcl_isfinite(point.normal_z);
178  const bool nonzero_normal = !ApproxZero(point.normal_x) || !ApproxZero(point.normal_y) || !ApproxZero(point.normal_z);
179  return finite_normal && nonzero_normal;
180 }
181 
182 template <typename PointIntensityType>
183 bool ValidIntensity(const PointIntensityType& point) {
184  return pcl_isfinite(point.intensity);
185 }
186 
187 template <typename PointType>
188 void RemoveInvalidAndZeroPoints(pcl::PointCloud<PointType>& cloud) {
189  size_t new_index = 0;
190  for (const auto& point : cloud.points) {
191  const bool valid_point = ValidPoint(point);
192  if (!valid_point) continue;
193  cloud.points[new_index++] = point;
194  }
195  if (new_index != cloud.points.size()) {
196  cloud.points.resize(new_index);
197  }
198 
199  cloud.height = 1;
200  cloud.width = static_cast<uint32_t>(new_index);
201  cloud.is_dense = true;
202 }
203 
204 template <typename PointType>
205 void ReplaceZerosWithNans(typename pcl::PointCloud<PointType>& cloud) {
206  for (auto& point : cloud.points) {
207  const bool nonzero_point = NonzeroPointXYZ(point);
208  if (nonzero_point) {
209  continue;
210  } else {
211  point.x = std::numeric_limits<double>::quiet_NaN();
212  point.y = std::numeric_limits<double>::quiet_NaN();
213  point.z = std::numeric_limits<double>::quiet_NaN();
214  }
215  }
216 }
217 
218 template <typename PointType>
219 typename pcl::PointCloud<PointType>::Ptr DownsamplePointCloud(const typename pcl::PointCloud<PointType>::Ptr cloud,
220  const double leaf_size) {
221  pcl::VoxelGrid<PointType> voxel_grid;
222  voxel_grid.setInputCloud(cloud);
223  voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);
224  typename pcl::PointCloud<PointType>::Ptr downsampled_cloud(new pcl::PointCloud<PointType>());
225  voxel_grid.filter(*downsampled_cloud);
226  return downsampled_cloud;
227 }
228 
229 template <typename PointType>
230 typename pcl::PointCloud<PointType>::Ptr BilateralFilterOrganizedCloud(
231  const typename pcl::PointCloud<PointType>::Ptr cloud, const double sigma_s, const double sigma_r) {
232  typename pcl::PointCloud<PointType>::Ptr downsampled_cloud(new pcl::PointCloud<PointType>());
233  typename pcl::FastBilateralFilter<PointType> bilateral_filter;
234  bilateral_filter.setInputCloud(cloud);
235  bilateral_filter.setSigmaS(sigma_s);
236  bilateral_filter.setSigmaR(sigma_r);
237  bilateral_filter.filter(*downsampled_cloud);
238  return downsampled_cloud;
239 }
240 
241 template <typename PointType>
242 void FilterCorrespondences(const typename pcl::PointCloud<PointType>& input_cloud,
243  const typename pcl::PointCloud<PointType>& target_cloud,
244  pcl::Correspondences& correspondences) {
245  for (auto correspondence_it = correspondences.begin(); correspondence_it != correspondences.end();) {
246  const auto& input_point = (input_cloud)[correspondence_it->index_query];
247  const auto& target_point = (target_cloud)[correspondence_it->index_match];
248  const bool invalid_correspondence = !std::isfinite(input_point.x) || !std::isfinite(input_point.y) ||
249  !std::isfinite(input_point.z) || !std::isfinite(target_point.x) ||
250  !std::isfinite(target_point.y) || !std::isfinite(target_point.z) ||
251  !std::isfinite(target_point.normal_x) ||
252  !std::isfinite(target_point.normal_y) || !std::isfinite(target_point.normal_z);
253  if (invalid_correspondence) {
254  correspondence_it = correspondences.erase(correspondence_it);
255  continue;
256  }
257  ++correspondence_it;
258  }
259 }
260 
261 template <typename PointType>
262 typename pcl::PointCloud<PointType>::Ptr FilteredPointCloud(
263  const typename pcl::PointCloud<PointType>::Ptr unfiltered_cloud) {
264  typename pcl::PointCloud<PointType>::Ptr filtered_cloud(new pcl::PointCloud<PointType>());
265  pcl::copyPointCloud(*unfiltered_cloud, *filtered_cloud);
266  RemoveInvalidAndZeroPoints(*filtered_cloud);
267  return filtered_cloud;
268 }
269 
270 template <typename PointType, typename PointWithNormalType>
271 void EstimateNormals(const typename pcl::PointCloud<PointType>::Ptr cloud, const double search_radius,
272  typename pcl::PointCloud<PointWithNormalType>& cloud_with_normals) {
273  typename pcl::NormalEstimation<PointType, pcl::Normal> ne;
274  ne.setInputCloud(cloud);
275  typename pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>());
276  ne.setSearchMethod(tree);
277  ne.setRadiusSearch(search_radius);
278  pcl::PointCloud<pcl::Normal> cloud_normals;
279  ne.compute(cloud_normals);
280  pcl::concatenateFields(*cloud, cloud_normals, cloud_with_normals);
281 }
282 
283 template <typename PointType, typename PointWithNormalType>
284 void EstimateOrganizedNormals(
285  const typename pcl::PointCloud<PointType>::Ptr cloud,
286  const typename pcl::IntegralImageNormalEstimation<PointType, pcl::Normal>::NormalEstimationMethod method,
287  const bool depth_dependent_smoothing, const double max_depth_change_factor, const double normal_smoothing_size,
288  typename pcl::PointCloud<PointWithNormalType>& cloud_with_normals) {
289  typename pcl::IntegralImageNormalEstimation<PointType, pcl::Normal> ne;
290  ne.setNormalEstimationMethod(method);
291  ne.setMaxDepthChangeFactor(max_depth_change_factor);
292  ne.setNormalSmoothingSize(normal_smoothing_size);
293  ne.setDepthDependentSmoothing(depth_dependent_smoothing);
294  ne.setInputCloud(cloud);
295  // Avoids issue where even though organized neighbor isn't used by normal estimation
296  // it attempts to estimate the camera intrinsics and sometimes fails
297  typename pcl::search::OrganizedNeighbor2<PointType>::Ptr organized_neighbor(
298  new pcl::search::OrganizedNeighbor2<PointType>());
299  ne.setSearchMethod(organized_neighbor);
300  pcl::PointCloud<pcl::Normal> cloud_normals;
301  ne.compute(cloud_normals);
302  pcl::concatenateFields(*cloud, cloud_normals, cloud_with_normals);
303 }
304 
305 template <typename PointType>
306 void NormalSpaceSubsampling(typename pcl::PointCloud<PointType>::Ptr cloud, const int bins_per_axis,
307  const int num_samples) {
308  typename pcl::NormalSpaceSampling<PointType, PointType> normal_space_sampling;
309  normal_space_sampling.setInputCloud(cloud);
310  normal_space_sampling.setNormals(cloud);
311  normal_space_sampling.setBins(bins_per_axis, bins_per_axis, bins_per_axis);
312  normal_space_sampling.setSample(num_samples);
313  normal_space_sampling.filter(*cloud);
314 }
315 
316 template <typename PointType, typename PointWithNormalType>
317 typename pcl::PointCloud<PointWithNormalType>::Ptr FilteredPointCloudWithNormals(
318  const typename pcl::PointCloud<PointType>::Ptr unfiltered_cloud, const double search_radius) {
319  typename pcl::PointCloud<PointType>::Ptr filtered_cloud = FilteredPointCloud<PointType>(unfiltered_cloud);
320  typename pcl::PointCloud<PointWithNormalType>::Ptr filtered_cloud_with_normals(
321  new pcl::PointCloud<PointWithNormalType>());
322  EstimateNormals<PointType, PointWithNormalType>(filtered_cloud, search_radius, *filtered_cloud_with_normals);
323  RemoveInvalidAndZeroPoints(*filtered_cloud_with_normals);
324  return filtered_cloud_with_normals;
325 }
326 
327 template <typename PointType>
328 Eigen::Vector3d Vector3d(const PointType& point) {
329  return Eigen::Vector3d(point.x, point.y, point.z);
330 }
331 
332 template <typename PointType>
333 Eigen::Vector3d NormalVector3d(const PointType& point_with_normal) {
334  return Eigen::Vector3d(point_with_normal.normal[0], point_with_normal.normal[1], point_with_normal.normal[2]);
335 }
336 
337 template <typename PointType>
338 boost::optional<Eigen::Vector3d> GetNormal(const Eigen::Vector3d& point, const pcl::PointCloud<PointType>& cloud,
339  const pcl::search::KdTree<PointType>& kdtree, const double search_radius) {
340  // Adapted from pcl code
341  PointType pcl_point;
342  pcl_point.x = point.x();
343  pcl_point.y = point.y();
344  pcl_point.z = point.z();
345 
346  std::vector<int> nn_indices;
347  std::vector<float> distances;
348  if (kdtree.radiusSearch(pcl_point, search_radius, nn_indices, distances, 0) < 3) {
349  LogDebug("GetNormal: Failed to get enough neighboring points for query point.");
350  return boost::none;
351  }
352 
353  float normal_x;
354  float normal_y;
355  float normal_z;
356  float curvature;
357  if (!computePointNormal(cloud, nn_indices, normal_x, normal_y, normal_z, curvature)) {
358  LogDebug("GetNormal: Failed to compute point normal.");
359  return boost::none;
360  }
361 
362  const double vpx = cloud.sensor_origin_.coeff(0);
363  const double vpy = cloud.sensor_origin_.coeff(1);
364  const double vpz = cloud.sensor_origin_.coeff(2);
365  flipNormalTowardsViewpoint(pcl_point, vpx, vpy, vpz, normal_x, normal_y, normal_z);
366  return Eigen::Vector3d(normal_x, normal_y, normal_z);
367 }
368 
369 template <typename PointType>
370 bool computePointNormal(const pcl::PointCloud<PointType>& cloud, const std::vector<int>& indices, float& normal_x,
371  float& normal_y, float& normal_z, float& curvature) {
372  // Adapted from pcl::common::centroid.h, should be a static or free function but is a member function instead
374  static EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_;
376  static Eigen::Vector4f xyz_centroid_;
377  if (indices.size() < 3 ||
378  pcl::computeMeanAndCovarianceMatrix(cloud, indices, covariance_matrix_, xyz_centroid_) == 0) {
379  return false;
380  }
381 
382  // Get the plane normal and surface curvature
383  pcl::solvePlaneParameters(covariance_matrix_, normal_x, normal_y, normal_z, curvature);
384  return true;
385 }
386 } // namespace point_cloud_common
387 #endif // POINT_CLOUD_COMMON_UTILITIES_H_
point_cloud_common
Definition: icp_correspondences.h:25
point_cloud_common::ValidIntensity
bool ValidIntensity(const PointIntensityType &point)
Definition: utilities.h:183
point_cloud_common::EstimateNormals
void EstimateNormals(const typename pcl::PointCloud< PointType >::Ptr cloud, const double search_radius, pcl::PointCloud< PointWithNormalType > &cloud_with_normals)
Definition: utilities.h:271
point_cloud_common::EstimateHistogramFeatures
pcl::PointCloud< pcl::FPFHSignature33 >::Ptr EstimateHistogramFeatures(const pcl::PointCloud< pcl::PointXYZINormal >::Ptr cloud_with_normals)
Definition: utilities.cc:37
logger.h
point_cloud_common::PointToPlaneCovariance
boost::optional< Eigen::Matrix< double, 6, 6 > > PointToPlaneCovariance(const std::vector< Eigen::Vector3d > &source_points, const std::vector< Eigen::Vector3d > &target_normals, const Eigen::Isometry3d &target_T_source)
Definition: utilities.cc:122
organized_neighbor2_impl.h
point_cloud_common::DownsamplePointCloud
pcl::PointCloud< PointType >::Ptr DownsamplePointCloud(const typename pcl::PointCloud< PointType >::Ptr cloud, const double leaf_size)
Definition: utilities.h:219
organized_neighbor2.h
point_cloud_common::RemoveInvalidAndZeroPoints
void RemoveInvalidAndZeroPoints(pcl::PointCloud< PointType > &cloud)
Definition: utilities.h:188
point_cloud_common::NormalVector3d
Eigen::Vector3d NormalVector3d(const PointType &point_with_normal)
Definition: utilities.h:333
point_cloud_common::RansacIA
Eigen::Matrix4f RansacIA(const pcl::PointCloud< pcl::PointXYZINormal >::Ptr source_cloud, const pcl::PointCloud< pcl::PointXYZINormal >::Ptr target_cloud)
Definition: utilities.cc:52
vision_common::Isometry3d
Eigen::Isometry3d Isometry3d(const cv::Mat &rodrigues_rotation_cv, const cv::Mat &translation_cv)
Definition: utilities.cc:58
point_cloud_common::Interpolate
pcl::PointXYZI Interpolate(const pcl::PointXYZI &point_a, const pcl::PointXYZI &point_b, const double alpha)
Definition: utilities.cc:136
point_cloud_common::FilteredPointCloudWithNormals
pcl::PointCloud< PointWithNormalType >::Ptr FilteredPointCloudWithNormals(const typename pcl::PointCloud< PointType >::Ptr unfiltered_cloud, const double search_radius)
Definition: utilities.h:317
point_cloud_common::Vector3d
Eigen::Vector3d Vector3d(const PointType &point)
Definition: utilities.h:328
point_cloud_common::ApproxZero
bool ApproxZero(const Type &point, const double epsilon=1e-5)
Definition: utilities.h:116
point_cloud_common::PointToPointJacobian
Eigen::Matrix< double, 3, 6 > PointToPointJacobian(const gtsam::Point3 &source_point, const gtsam::Pose3 &target_T_source)
Definition: utilities.cc:82
point_cloud_common::FilterCorrespondences
void FilterCorrespondences(const typename pcl::PointCloud< PointType > &input_cloud, const typename pcl::PointCloud< PointType > &target_cloud, pcl::Correspondences &correspondences)
Definition: utilities.h:242
point_cloud_common::NormalSpaceSubsampling
void NormalSpaceSubsampling(typename pcl::PointCloud< PointType >::Ptr cloud, const int bins_per_axis, const int num_samples)
Definition: utilities.h:306
point_cloud_common::RelativeTransformUmeyama
Eigen::Isometry3d RelativeTransformUmeyama(const std::vector< Eigen::Vector3d > &source_points, const std::vector< Eigen::Vector3d > &target_points)
Definition: utilities.cc:89
point_cloud_common::FilteredPointCloud
pcl::PointCloud< PointType >::Ptr FilteredPointCloud(const typename pcl::PointCloud< PointType >::Ptr unfiltered_cloud)
Definition: utilities.h:262
point_cloud_common::BilateralFilterOrganizedCloud
pcl::PointCloud< PointType >::Ptr BilateralFilterOrganizedCloud(const typename pcl::PointCloud< PointType >::Ptr cloud, const double sigma_s, const double sigma_r)
Definition: utilities.h:230
point_cloud_common::PointToPointCovariance
boost::optional< Eigen::Matrix< double, 6, 6 > > PointToPointCovariance(const std::vector< Eigen::Vector3d > &source_points, const Eigen::Isometry3d &target_T_source)
Definition: utilities.cc:110
point_cloud_common::EstimateOrganizedNormals
void EstimateOrganizedNormals(const typename pcl::PointCloud< PointType >::Ptr cloud, const typename pcl::IntegralImageNormalEstimation< PointType, pcl::Normal >::NormalEstimationMethod method, const bool depth_dependent_smoothing, const double max_depth_change_factor, const double normal_smoothing_size, pcl::PointCloud< PointWithNormalType > &cloud_with_normals)
Definition: utilities.h:284
LogDebug
#define LogDebug(msg)
Definition: logger.h:69
point_cloud_common::NonzeroPointXYZ
bool NonzeroPointXYZ(const PointXYZType &point)
Definition: utilities.h:162
pcl::search::OrganizedNeighbor2::Ptr
boost::shared_ptr< pcl::search::OrganizedNeighbor2< PointT > > Ptr
Definition: organized_neighbor2.h:69
pcl::search::OrganizedNeighbor2
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized_neighbor2.h:60
point_cloud_common::ReplaceZerosWithNans
void ReplaceZerosWithNans(typename pcl::PointCloud< PointType > &cloud)
Definition: utilities.h:205
point_cloud_common::computePointNormal
bool computePointNormal(const pcl::PointCloud< PointType > &cloud, const std::vector< int > &indices, float &normal_x, float &normal_y, float &normal_z, float &curvature)
Definition: utilities.h:370
point_cloud_common::GetNormal
boost::optional< Eigen::Vector3d > GetNormal(const Eigen::Vector3d &point, const pcl::PointCloud< PointType > &cloud, const pcl::search::KdTree< PointType > &kdtree, const double search_radius=0.03)
Definition: utilities.h:338
point_cloud_common::ValidNormal
bool ValidNormal(const PointNormalType &point)
Definition: utilities.h:175
point_cloud_common::ValidPointXYZ
bool ValidPointXYZ(const PointXYZType &point)
Definition: utilities.h:168
point_cloud_common::ValidPoint
bool ValidPoint(const PointType &point)=delete
point_cloud_common::PointToPlaneJacobian
Eigen::Matrix< double, 1, 6 > PointToPlaneJacobian(const gtsam::Point3 &source_point, const gtsam::Vector3 &normal, const gtsam::Pose3 &target_T_source)
Definition: utilities.cc:75