Point Cloud Library (PCL)  1.12.1-dev
gicp6d.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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  */
38 
39 #pragma once
40 
41 #include <pcl/kdtree/impl/kdtree_flann.hpp>
42 #include <pcl/registration/gicp.h>
43 #include <pcl/memory.h>
44 #include <pcl/pcl_exports.h> // for PCL_EXPORTS
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_representation.h>
47 #include <pcl/point_types.h>
48 
49 namespace pcl {
50 /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information
51  * into the Generalized Iterative Closest Point (GICP) algorithm.
52  *
53  * The suggested input is PointXYZRGBA.
54  *
55  * \note If you use this code in any academic work, please cite:
56  *
57  * - M. Korn, M. Holzkothen, J. Pauli
58  * Color Supported Generalized-ICP.
59  * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory
60  * and Applications, Lisbon, Portugal, January 2014.
61  *
62  * \author Martin Holzkothen, Michael Korn
63  * \ingroup registration
64  */
66 : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> {
67  using PointSource = PointXYZRGBA;
68  using PointTarget = PointXYZRGBA;
69 
70 public:
71  /** \brief constructor.
72  *
73  * \param[in] lab_weight the color weight
74  */
75  GeneralizedIterativeClosestPoint6D(float lab_weight = 0.032f);
76 
77  /** \brief Provide a pointer to the input source
78  * (e.g., the point cloud that we want to align to the target)
79  *
80  * \param[in] cloud the input point cloud source
81  */
82  void
83  setInputSource(const PointCloudSourceConstPtr& cloud) override;
84 
85  /** \brief Provide a pointer to the input target
86  * (e.g., the point cloud that we want to align the input source to)
87  *
88  * \param[in] target the input point cloud target
89  */
90  void
91  setInputTarget(const PointCloudTargetConstPtr& target) override;
92 
93 protected:
94  /** \brief Rigid transformation computation method with initial guess.
95  * \param output the transformed input point cloud dataset using the rigid
96  * transformation found \param guess the initial guess of the transformation to
97  * compute
98  */
99  void
101  const Eigen::Matrix4f& guess) override;
102 
103  /** \brief Search for the closest nearest neighbor of a given point.
104  * \param query the point to search a nearest neighbour for
105  * \param index vector of size 1 to store the index of the nearest neighbour found
106  * \param distance vector of size 1 to store the distance to nearest neighbour found
107  */
108  inline bool
110  pcl::Indices& index,
111  std::vector<float>& distance);
112 
113 protected:
114  /** \brief Holds the converted (LAB) data cloud. */
116 
117  /** \brief Holds the converted (LAB) model cloud. */
119 
120  /** \brief 6d-tree to search in model cloud. */
122 
123  /** \brief The color weight. */
124  float lab_weight_;
125 
126  /** \brief Custom point representation to perform kdtree searches in more than 3
127  * (i.e. in all 6) dimensions. */
128  class MyPointRepresentation : public PointRepresentation<PointXYZLAB> {
131 
132  public:
133  using Ptr = shared_ptr<MyPointRepresentation>;
134  using ConstPtr = shared_ptr<const MyPointRepresentation>;
135 
137  {
138  nr_dimensions_ = 6;
139  trivial_ = false;
140  }
141 
142  ~MyPointRepresentation() override = default;
143 
144  inline Ptr
145  makeShared() const
146  {
147  return Ptr(new MyPointRepresentation(*this));
148  }
149 
150  void
151  copyToFloatArray(const PointXYZLAB& p, float* out) const override
152  {
153  // copy all of the six values
154  out[0] = p.x;
155  out[1] = p.y;
156  out[2] = p.z;
157  out[3] = p.L;
158  out[4] = p.a;
159  out[5] = p.b;
160  }
161  };
162 
163  /** \brief Enables 6d searches with kd-tree class using the color weight. */
165 };
166 } // namespace pcl
Custom point representation to perform kdtree searches in more than 3 (i.e.
Definition: gicp6d.h:128
void copyToFloatArray(const PointXYZLAB &p, float *out) const override
Copy point data from input point to a float array.
Definition: gicp6d.h:151
shared_ptr< const MyPointRepresentation > ConstPtr
Definition: gicp6d.h:134
GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the Generalized Ite...
Definition: gicp6d.h:66
bool searchForNeighbors(const PointXYZLAB &query, pcl::Indices &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
void setInputTarget(const PointCloudTargetConstPtr &target) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
MyPointRepresentation point_rep_
Enables 6d searches with kd-tree class using the color weight.
Definition: gicp6d.h:164
float lab_weight_
The color weight.
Definition: gicp6d.h:124
pcl::PointCloud< PointXYZLAB >::Ptr cloud_lab_
Holds the converted (LAB) data cloud.
Definition: gicp6d.h:115
KdTreeFLANN< PointXYZLAB > target_tree_lab_
6d-tree to search in model cloud.
Definition: gicp6d.h:121
GeneralizedIterativeClosestPoint6D(float lab_weight=0.032f)
constructor.
pcl::PointCloud< PointXYZLAB >::Ptr target_lab_
Holds the converted (LAB) model cloud.
Definition: gicp6d.h:118
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition: gicp.h:59
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: registration.h:78
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: registration.h:82
Defines all the PCL implemented PointT point type structures.
Defines functions, macros and traits for allocating and using memory.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates, and the CIELAB color.
A point structure representing Euclidean xyz coordinates, and the RGBA color.