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