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