correspondence_estimation_organized_projection.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* 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.
*
* $Id$
*
*/
#pragma once
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
namespace pcl {
namespace registration {
/** \brief CorrespondenceEstimationOrganizedProjection computes correspondences by
* projecting the source point cloud onto the target point cloud using the camera
* intrinsic and extrinsic parameters. The correspondences can be trimmed by a depth
* threshold and by a distance threshold. It is not as precise as a nearest neighbor
* search, but it is much faster, as it avoids the usage of any additional structures
* (i.e., kd-trees). \note The target point cloud must be organized (no restrictions on
* the source) and the target point cloud must be given in the camera coordinate frame.
* Any other transformation is specified by the src_to_tgt_transformation_ variable.
* \author Alex Ichim
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimationOrganizedProjection
: public CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> {
public:
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
input_transformed_;
using PCLBase<PointSource>::deinitCompute;
using PCLBase<PointSource>::input_;
using PCLBase<PointSource>::indices_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::getClassName;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
point_representation_;
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::
target_cloud_updated_;
using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget = pcl::PointCloud<PointTarget>;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using Ptr = shared_ptr<
CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const CorrespondenceEstimationOrganizedProjection<PointSource,
PointTarget,
Scalar>>;
/** \brief Empty constructor that sets all the intrinsic calibration to the default
* Kinect values. */
CorrespondenceEstimationOrganizedProjection()
: fx_(525.f)
, fy_(525.f)
, cx_(319.5f)
, cy_(239.5f)
, src_to_tgt_transformation_(Eigen::Matrix4f::Identity())
, depth_threshold_(std::numeric_limits<float>::max())
, projection_matrix_(Eigen::Matrix3f::Identity())
{}
/** \brief Sets the focal length parameters of the target camera.
* \param[in] fx the focal length in pixels along the x-axis of the image
* \param[in] fy the focal length in pixels along the y-axis of the image
*/
inline void
setFocalLengths(const float fx, const float fy)
{
fx_ = fx;
fy_ = fy;
}
/** \brief Reads back the focal length parameters of the target camera.
* \param[out] fx the focal length in pixels along the x-axis of the image
* \param[out] fy the focal length in pixels along the y-axis of the image
*/
inline void
getFocalLengths(float& fx, float& fy) const
{
fx = fx_;
fy = fy_;
}
/** \brief Sets the camera center parameters of the target camera.
* \param[in] cx the x-coordinate of the camera center
* \param[in] cy the y-coordinate of the camera center
*/
inline void
setCameraCenters(const float cx, const float cy)
{
cx_ = cx;
cy_ = cy;
}
/** \brief Reads back the camera center parameters of the target camera.
* \param[out] cx the x-coordinate of the camera center
* \param[out] cy the y-coordinate of the camera center
*/
inline void
getCameraCenters(float& cx, float& cy) const
{
cx = cx_;
cy = cy_;
}
/** \brief Sets the transformation from the source point cloud to the target point
* cloud. \note The target point cloud must be in its local camera coordinates, so use
* this transformation to correct for that. \param[in] src_to_tgt_transformation the
* transformation
*/
inline void
setSourceTransformation(const Eigen::Matrix4f& src_to_tgt_transformation)
{
src_to_tgt_transformation_ = src_to_tgt_transformation;
}
/** \brief Reads back the transformation from the source point cloud to the target
* point cloud. \note The target point cloud must be in its local camera coordinates,
* so use this transformation to correct for that. \return the transformation
*/
inline Eigen::Matrix4f
getSourceTransformation() const
{
return (src_to_tgt_transformation_);
}
/** \brief Sets the depth threshold; after projecting the source points in the image
* space of the target camera, this threshold is applied on the depths of
* corresponding dexels to eliminate the ones that are too far from each other.
* \param[in] depth_threshold the depth threshold
*/
inline void
setDepthThreshold(const float depth_threshold)
{
depth_threshold_ = depth_threshold;
}
/** \brief Reads back the depth threshold; after projecting the source points in the
* image space of the target camera, this threshold is applied on the depths of
* corresponding dexels to eliminate the ones that are too far from each other.
* \return the depth threshold
*/
inline float
getDepthThreshold() const
{
return (depth_threshold_);
}
/** \brief Computes the correspondences, applying a maximum Euclidean distance
* threshold.
* \param[out] correspondences the found correspondences (index of query point, index
* of target point, distance)
* \param[in] max_distance Euclidean distance threshold above which correspondences
* will be rejected
*/
void
determineCorrespondences(Correspondences& correspondences, double max_distance);
/** \brief Computes the correspondences, applying a maximum Euclidean distance
* threshold.
* \param[out] correspondences the found correspondences (index of query and target
* point, distance)
* \param[in] max_distance Euclidean distance threshold above which correspondences
* will be rejected
*/
void
determineReciprocalCorrespondences(Correspondences& correspondences,
double max_distance);
/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
clone() const
{
Ptr copy(new CorrespondenceEstimationOrganizedProjection<PointSource,
PointTarget,
Scalar>(*this));
return (copy);
}
protected:
using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_;
bool
initCompute();
float fx_, fy_;
float cx_, cy_;
Eigen::Matrix4f src_to_tgt_transformation_;
float depth_threshold_;
Eigen::Matrix3f projection_matrix_;
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace registration
} // namespace pcl
#include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:70
pcl::PointCloud< PointSource >
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud< PointSource >::ConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:414
pcl::registration::CorrespondenceEstimationBase
Abstract CorrespondenceEstimationBase class.
Definition: correspondence_estimation.h:60
pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float >::input_transformed_
PointCloudTargetPtr input_transformed_
The transformed input source point cloud dataset.
Definition: correspondence_estimation.h:312
pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float >::point_representation_
PointRepresentationConstPtr point_representation_
The point representation used (internal).
Definition: correspondence_estimation.h:309
pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float >::target_cloud_updated_
bool target_cloud_updated_
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
Definition: correspondence_estimation.h:335
pcl::registration::CorrespondenceEstimationBase::Ptr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation.h:63
pcl::registration::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
Definition: correspondence_estimation_organized_projection.h:61
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: correspondence_estimation_organized_projection.h:80
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: correspondence_estimation_organized_projection.h:77
pcl::registration::CorrespondenceEstimationOrganizedProjection::fx_
float fx_
Definition: correspondence_estimation_organized_projection.h:226
pcl::registration::CorrespondenceEstimationOrganizedProjection::clone
virtual CorrespondenceEstimationBase< PointSource, PointTarget, Scalar >::Ptr clone() const
Clone and cast to CorrespondenceEstimationBase.
Definition: correspondence_estimation_organized_projection.h:212
pcl::registration::CorrespondenceEstimationOrganizedProjection::setFocalLengths
void setFocalLengths(const float fx, const float fy)
Sets the focal length parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:107
pcl::registration::CorrespondenceEstimationOrganizedProjection::cx_
float cx_
Definition: correspondence_estimation_organized_projection.h:227
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: correspondence_estimation_organized_projection.h:76
pcl::registration::CorrespondenceEstimationOrganizedProjection::Ptr
shared_ptr< CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation_organized_projection.h:84
pcl::registration::CorrespondenceEstimationOrganizedProjection::determineCorrespondences
void determineCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Definition: correspondence_estimation_organized_projection.hpp:78
pcl::registration::CorrespondenceEstimationOrganizedProjection::getCameraCenters
void getCameraCenters(float &cx, float &cy) const
Reads back the camera center parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:140
pcl::registration::CorrespondenceEstimationOrganizedProjection::initCompute
bool initCompute()
Definition: correspondence_estimation_organized_projection.hpp:51
pcl::registration::CorrespondenceEstimationOrganizedProjection::getSourceTransformation
Eigen::Matrix4f getSourceTransformation() const
Reads back the transformation from the source point cloud to the target point cloud.
Definition: correspondence_estimation_organized_projection.h:162
pcl::registration::CorrespondenceEstimationOrganizedProjection::cy_
float cy_
Definition: correspondence_estimation_organized_projection.h:227
pcl::registration::CorrespondenceEstimationOrganizedProjection::setSourceTransformation
void setSourceTransformation(const Eigen::Matrix4f &src_to_tgt_transformation)
Sets the transformation from the source point cloud to the target point cloud.
Definition: correspondence_estimation_organized_projection.h:152
pcl::registration::CorrespondenceEstimationOrganizedProjection::src_to_tgt_transformation_
Eigen::Matrix4f src_to_tgt_transformation_
Definition: correspondence_estimation_organized_projection.h:228
pcl::registration::CorrespondenceEstimationOrganizedProjection::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
Definition: correspondence_estimation_organized_projection.h:92
pcl::registration::CorrespondenceEstimationOrganizedProjection::ConstPtr
shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
Definition: correspondence_estimation_organized_projection.h:88
pcl::registration::CorrespondenceEstimationOrganizedProjection::projection_matrix_
Eigen::Matrix3f projection_matrix_
Definition: correspondence_estimation_organized_projection.h:231
pcl::registration::CorrespondenceEstimationOrganizedProjection::getDepthThreshold
float getDepthThreshold() const
Reads back the depth threshold; after projecting the source points in the image space of the target c...
Definition: correspondence_estimation_organized_projection.h:184
pcl::registration::CorrespondenceEstimationOrganizedProjection::determineReciprocalCorrespondences
void determineReciprocalCorrespondences(Correspondences &correspondences, double max_distance)
Computes the correspondences, applying a maximum Euclidean distance threshold.
Definition: correspondence_estimation_organized_projection.hpp:123
pcl::registration::CorrespondenceEstimationOrganizedProjection::depth_threshold_
float depth_threshold_
Definition: correspondence_estimation_organized_projection.h:229
pcl::registration::CorrespondenceEstimationOrganizedProjection::setCameraCenters
void setCameraCenters(const float cx, const float cy)
Sets the camera center parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:129
pcl::registration::CorrespondenceEstimationOrganizedProjection::fy_
float fy_
Definition: correspondence_estimation_organized_projection.h:226
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: correspondence_estimation_organized_projection.h:81
pcl::registration::CorrespondenceEstimationOrganizedProjection::getFocalLengths
void getFocalLengths(float &fx, float &fy) const
Reads back the focal length parameters of the target camera.
Definition: correspondence_estimation_organized_projection.h:118
pcl::registration::CorrespondenceEstimationOrganizedProjection::setDepthThreshold
void setDepthThreshold(const float depth_threshold)
Sets the depth threshold; after projecting the source points in the image space of the target camera,...
Definition: correspondence_estimation_organized_projection.h:173
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
memory.h
Defines functions, macros and traits for allocating and using memory.
Eigen
Definition: bfgs.h:10
pcl
Definition: convolution.h:46
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:89
pcl_macros.h
Defines all the PCL and non-PCL macros used.