gicp6d.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#pragma once
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/registration/gicp.h>
namespace pcl
{
struct EIGEN_ALIGN16 _PointXYZLAB
{
PCL_ADD_POINT4D; // this adds the members x,y,z
union
{
struct
{
float L;
float a;
float b;
};
float data_lab[4];
};
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
/** \brief A custom point type for position and CIELAB color value */
struct PointXYZLAB : public _PointXYZLAB
{
inline PointXYZLAB ()
{
x = y = z = 0.0f; data[3] = 1.0f; // important for homogeneous coordinates
L = a = b = 0.0f; data_lab[3] = 0.0f;
}
};
}
// register the custom point type in PCL
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)
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
namespace pcl
{
/** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information into the
* Generalized Iterative Closest Point (GICP) algorithm.
*
* The suggested input is PointXYZRGBA.
*
* \note If you use this code in any academic work, please cite:
*
* - M. Korn, M. Holzkothen, J. Pauli
* Color Supported Generalized-ICP.
* In Proceedings of VISAPP 2014 - International Conference on Computer Vision Theory and Applications,
* Lisbon, Portugal, January 2014.
*
* \author Martin Holzkothen, Michael Korn
* \ingroup registration
*/
class PCL_EXPORTS GeneralizedIterativeClosestPoint6D : public GeneralizedIterativeClosestPoint<PointXYZRGBA, PointXYZRGBA>
{
using PointSource = PointXYZRGBA;
using PointTarget = PointXYZRGBA;
public:
/** \brief constructor.
*
* \param[in] lab_weight the color weight
*/
GeneralizedIterativeClosestPoint6D (float lab_weight = 0.032f);
/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud source
*/
void
setInputSource (const PointCloudSourceConstPtr& cloud) override;
/** \brief Provide a pointer to the input target
* (e.g., the point cloud that we want to align the input source to)
*
* \param[in] cloud the input point cloud target
*/
void
setInputTarget (const PointCloudTargetConstPtr& target) override;
protected:
/** \brief Rigid transformation computation method with initial guess.
* \param output the transformed input point cloud dataset using the rigid transformation found
* \param guess the initial guess of the transformation to compute
*/
void
computeTransformation (PointCloudSource& output,
const Eigen::Matrix4f& guess) override;
/** \brief Search for the closest nearest neighbor of a given point.
* \param query the point to search a nearest neighbour for
* \param index vector of size 1 to store the index of the nearest neighbour found
* \param distance vector of size 1 to store the distance to nearest neighbour found
*/
inline bool
searchForNeighbors (const PointXYZLAB& query, std::vector<int>& index, std::vector<float>& distance);
protected:
/** \brief Holds the converted (LAB) data cloud. */
pcl::PointCloud<PointXYZLAB>::Ptr cloud_lab_;
/** \brief Holds the converted (LAB) model cloud. */
pcl::PointCloud<PointXYZLAB>::Ptr target_lab_;
/** \brief 6d-tree to search in model cloud. */
KdTreeFLANN<PointXYZLAB> target_tree_lab_;
/** \brief The color weight. */
float lab_weight_;
/** \brief Custom point representation to perform kdtree searches in more than 3 (i.e. in all 6) dimensions. */
class MyPointRepresentation : public PointRepresentation<PointXYZLAB>
{
using PointRepresentation<PointXYZLAB>::nr_dimensions_;
using PointRepresentation<PointXYZLAB>::trivial_;
public:
using Ptr = shared_ptr<MyPointRepresentation>;
using ConstPtr = shared_ptr<const MyPointRepresentation>;
MyPointRepresentation ()
{
nr_dimensions_ = 6;
trivial_ = false;
}
~MyPointRepresentation ()
{
}
inline Ptr
makeShared () const
{
return Ptr (new MyPointRepresentation (*this));
}
void
copyToFloatArray (const PointXYZLAB &p, float * out) const override
{
// copy all of the six values
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.L;
out[4] = p.a;
out[5] = p.b;
}
};
/** \brief Enables 6d searches with kd-tree class using the color weight. */
MyPointRepresentation point_rep_;
};
}
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:78
pcl::_PointXYZLAB::PCL_ADD_POINT4D
PCL_ADD_POINT4D
Definition: gicp6d.h:52
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:371
pcl::PointRepresentation::trivial_
bool trivial_
Indicates whether this point representation is trivial.
Definition: point_representation.h:73
pcl::_PointXYZLAB
Definition: gicp6d.h:50
pcl::_PointXYZLAB::L
float L
Definition: gicp6d.h:57
pcl::_PointXYZLAB::a
float a
Definition: gicp6d.h:58
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::PointRepresentation::nr_dimensions_
int nr_dimensions_
The number of dimensions in this point's vector (i.e.
Definition: point_representation.h:61
pcl::PointXYZLAB::PointXYZLAB
PointXYZLAB()
Definition: gicp6d.h:69
pcl::_PointXYZLAB::data_lab
float data_lab[4]
Definition: gicp6d.h:61
pcl::_PointXYZLAB::b
float b
Definition: gicp6d.h:59
pcl::PointXYZLAB
A custom point type for position and CIELAB color value.
Definition: gicp6d.h:67
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:253