Point Cloud Library (PCL)  1.11.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/memory.h>
42 #include <pcl/pcl_exports.h> // for PCL_EXPORTS
43 #include <pcl/point_types.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_representation.h>
46 #include <pcl/kdtree/impl/kdtree_flann.hpp>
47 #include <pcl/registration/gicp.h>
48 
49 namespace pcl
50 {
51  struct EIGEN_ALIGN16 _PointXYZLAB
52  {
53  PCL_ADD_POINT4D; // this adds the members x,y,z
54  union
55  {
56  struct
57  {
58  float L;
59  float a;
60  float b;
61  };
62  float data_lab[4];
63  };
65  };
66 
67  /** \brief A custom point type for position and CIELAB color value */
68  struct PointXYZLAB : public _PointXYZLAB
69  {
70  inline PointXYZLAB ()
71  {
72  x = y = z = 0.0f; data[3] = 1.0f; // important for homogeneous coordinates
73  L = a = b = 0.0f; data_lab[3] = 0.0f;
74  }
75  };
76 }
77 
78 // register the custom point type in PCL
80  (float, x, x)
81  (float, y, y)
82  (float, z, z)
83  (float, L, L)
84  (float, a, a)
85  (float, b, b)
86 )
87 POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
88 
89 namespace pcl
90 {
91  /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the
92  * Generalized Iterative Closest Point (GICP) algorithm.
93  *
94  * The suggested input is PointXYZRGBA.
95  *
96  * \note If you use this code in any academic work, please cite:
97  *
98  * - M. Korn, M. Holzkothen, J. Pauli
99  * Color Supported Generalized-ICP.
100  * In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory and Applications,
101  * Lisbon, Portugal, January 2014.
102  *
103  * \author Martin Holzkothen, Michael Korn
104  * \ingroup registration
105  */
106  class PCL_EXPORTS GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA>
107  {
108  using PointSource = PointXYZRGBA;
109  using PointTarget = PointXYZRGBA;
110 
111  public:
112 
113  /** \brief constructor.
114  *
115  * \param[in] lab_weight the color weight
116  */
117  GeneralizedIterativeClosestPoint6D (float lab_weight = 0.032f);
118 
119  /** \brief Provide a pointer to the input source
120  * (e.g., the point cloud that we want to align to the target)
121  *
122  * \param[in] cloud the input point cloud source
123  */
124  void
125  setInputSource (const PointCloudSourceConstPtr& cloud) override;
126 
127  /** \brief Provide a pointer to the input target
128  * (e.g., the point cloud that we want to align the input source to)
129  *
130  * \param[in] cloud the input point cloud target
131  */
132  void
133  setInputTarget (const PointCloudTargetConstPtr& target) override;
134 
135  protected:
136 
137  /** \brief Rigid transformation computation method with initial guess.
138  * \param output the transformed input point cloud dataset using the rigid transformation found
139  * \param guess the initial guess of the transformation to compute
140  */
141  void
142  computeTransformation (PointCloudSource& output,
143  const Eigen::Matrix4f& guess) override;
144 
145  /** \brief Search for the closest nearest neighbor of a given point.
146  * \param query the point to search a nearest neighbour for
147  * \param index vector of size 1 to store the index of the nearest neighbour found
148  * \param distance vector of size 1 to store the distance to nearest neighbour found
149  */
150  inline bool
151  searchForNeighbors (const PointXYZLAB& query, std::vector<int>& index, std::vector<float>& distance);
152 
153  protected:
154  /** \brief Holds the converted (LAB) data cloud. */
156 
157  /** \brief Holds the converted (LAB) model cloud. */
159 
160  /** \brief 6d-tree to search in model cloud. */
161  KdTreeFLANN<PointXYZLAB> target_tree_lab_;
162 
163  /** \brief The color weight. */
164  float lab_weight_;
165 
166  /** \brief Custom point representation to perform kdtree searches in more than 3 (i.e. in all 6) dimensions. */
167  class MyPointRepresentation : public PointRepresentation<PointXYZLAB>
168  {
171 
172  public:
173  using Ptr = shared_ptr<MyPointRepresentation>;
174  using ConstPtr = shared_ptr<const MyPointRepresentation>;
175 
176  MyPointRepresentation ()
177  {
178  nr_dimensions_ = 6;
179  trivial_ = false;
180  }
181 
182 
183  ~MyPointRepresentation ()
184  {
185  }
186 
187  inline Ptr
188  makeShared () const
189  {
190  return Ptr (new MyPointRepresentation (*this));
191  }
192 
193  void
194  copyToFloatArray (const PointXYZLAB &p, float * out) const override
195  {
196  // copy all of the six values
197  out[0] = p.x;
198  out[1] = p.y;
199  out[2] = p.z;
200  out[3] = p.L;
201  out[4] = p.a;
202  out[5] = p.b;
203  }
204  };
205 
206  /** \brief Enables 6d searches with kd-tree class using the color weight. */
207  MyPointRepresentation point_rep_;
208  };
209 }
pcl
Definition: convolution.h:46
point_types.h
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
POINT_CLOUD_REGISTER_POINT_STRUCT
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)) namespace pcl
Definition: gicp6d.h:79
pcl::_PointXYZLAB::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: gicp6d.h:53
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
pcl::PointRepresentation::trivial_
bool trivial_
Indicates whether this point representation is trivial.
Definition: point_representation.h:75
pcl::_PointXYZLAB
Definition: gicp6d.h:51
pcl::_PointXYZLAB::L
float L
Definition: gicp6d.h:58
pcl::_PointXYZLAB::a
float a
Definition: gicp6d.h:59
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:406
pcl::PointRepresentation::nr_dimensions_
int nr_dimensions_
The number of dimensions in this point's vector (i.e.
Definition: point_representation.h:63
pcl::PointXYZLAB::PointXYZLAB
PointXYZLAB()
Definition: gicp6d.h:70
pcl::_PointXYZLAB::data_lab
float data_lab[4]
Definition: gicp6d.h:62
pcl::_PointXYZLAB::b
float b
Definition: gicp6d.h:60
pcl::PointXYZLAB
A custom point type for position and CIELAB color value.
Definition: gicp6d.h:68
memory.h
Defines functions, macros and traits for allocating and using memory.
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:323