Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
transformation_validation_euclidean.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
42#define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
43
44namespace pcl {
45
46namespace registration {
47
48template <typename PointSource, typename PointTarget, typename Scalar>
49double
52 const PointCloudTargetConstPtr& cloud_tgt,
53 const Matrix4& transformation_matrix) const
54{
55 double fitness_score = 0.0;
56
57 // Transform the input dataset using the final transformation
58 pcl::PointCloud<PointSource> input_transformed;
59 // transformPointCloud (*cloud_src, input_transformed, transformation_matrix);
60 input_transformed.resize(cloud_src->size());
61 for (std::size_t i = 0; i < cloud_src->size(); ++i) {
62 const PointSource& src = (*cloud_src)[i];
63 PointTarget& tgt = input_transformed[i];
64 tgt.x = static_cast<float>(
65 transformation_matrix(0, 0) * src.x + transformation_matrix(0, 1) * src.y +
66 transformation_matrix(0, 2) * src.z + transformation_matrix(0, 3));
67 tgt.y = static_cast<float>(
68 transformation_matrix(1, 0) * src.x + transformation_matrix(1, 1) * src.y +
69 transformation_matrix(1, 2) * src.z + transformation_matrix(1, 3));
70 tgt.z = static_cast<float>(
71 transformation_matrix(2, 0) * src.x + transformation_matrix(2, 1) * src.y +
72 transformation_matrix(2, 2) * src.z + transformation_matrix(2, 3));
73 }
74
76 if (!force_no_recompute_) {
77 tree_->setPointRepresentation(point_rep);
78 tree_->setInputCloud(cloud_tgt);
79 }
80
81 pcl::Indices nn_indices(1);
82 std::vector<float> nn_dists(1);
83
84 // For each point in the source dataset
85 int nr = 0;
86 for (const auto& point : input_transformed) {
87 // Find its nearest neighbor in the target
88 tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
89
90 // Deal with occlusions (incomplete targets)
91 if (nn_dists[0] > max_range_)
92 continue;
93
94 // Calculate the fitness score
95 fitness_score += nn_dists[0];
96 ++nr;
97 }
98
99 if (nr > 0)
100 return (fitness_score / nr);
101 return (std::numeric_limits<double>::max());
102}
103
104} // namespace registration
105} // namespace pcl
106
107#endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
typename TransformationValidation< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
typename TransformationValidation< PointSource, PointTarget >::PointCloudTargetConstPtr PointCloudTargetConstPtr
double validateTransformation(const PointCloudSourceConstPtr &cloud_src, const PointCloudTargetConstPtr &cloud_tgt, const Matrix4 &transformation_matrix) const
Validate the given transformation with respect to the input cloud data, and return a score.
typename TransformationValidation< PointSource, PointTarget >::PointCloudSourceConstPtr PointCloudSourceConstPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133