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