NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
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