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$
*
*/
#ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
#define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_
#include <pcl/registration/correspondence_estimation.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
*/
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_;
typedef pcl::PointCloud<PointSource> PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
typedef pcl::PointCloud<PointTarget> PointCloudTarget;
typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
typedef boost::shared_ptr< CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > Ptr;
typedef boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar> > ConstPtr;
/** \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 correspondences
* \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 correspondences
* \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 boost::shared_ptr< CorrespondenceEstimationBase<PointSource, PointTarget, Scalar> >
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:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
}
#include <pcl/registration/impl/correspondence_estimation_organized_projection.hpp>
#endif /* PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_H_ */
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:71
pcl::registration::CorrespondenceEstimationOrganizedProjection::depth_threshold_
float depth_threshold_
Definition: correspondence_estimation_organized_projection.h:198
pcl::registration::CorrespondenceEstimationOrganizedProjection::projection_matrix_
Eigen::Matrix3f projection_matrix_
Definition: correspondence_estimation_organized_projection.h:200
pcl
Definition: convolution.h:45
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:111
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudTargetConstPtr
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: correspondence_estimation_organized_projection.h:79
std
Definition: multi_grid_octree_data.hpp:45
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:155
pcl::registration::CorrespondenceEstimationOrganizedProjection::cy_
float cy_
Definition: correspondence_estimation_organized_projection.h:196
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:103
pcl::registration::CorrespondenceEstimationOrganizedProjection::initCompute
bool initCompute()
Definition: correspondence_estimation_organized_projection.hpp:46
Eigen
Definition: bfgs.h:10
pcl::registration::CorrespondenceEstimationOrganizedProjection::fx_
float fx_
Definition: correspondence_estimation_organized_projection.h:195
pcl::PointCloud< PointSource >::Ptr
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:428
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudSourceConstPtr
PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: correspondence_estimation_organized_projection.h:75
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:164
pcl::registration::CorrespondenceEstimationOrganizedProjection::fy_
float fy_
Definition: correspondence_estimation_organized_projection.h:195
pcl::registration::CorrespondenceEstimationOrganizedProjection::src_to_tgt_transformation_
Eigen::Matrix4f src_to_tgt_transformation_
Definition: correspondence_estimation_organized_projection.h:197
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:68
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudTargetPtr
PointCloudTarget::Ptr PointCloudTargetPtr
Definition: correspondence_estimation_organized_projection.h:78
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:92
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:128
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:120
pcl::PointCloud< PointSource >::ConstPtr
boost::shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:429
pcl::registration::CorrespondenceEstimationOrganizedProjection::cx_
float cx_
Definition: correspondence_estimation_organized_projection.h:196
pcl::registration::CorrespondenceEstimationOrganizedProjection::Ptr
boost::shared_ptr< CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation_organized_projection.h:81
pcl::PointCloud< PointSource >
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudTarget
pcl::PointCloud< PointTarget > PointCloudTarget
Definition: correspondence_estimation_organized_projection.h:77
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:118
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:146
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudSourcePtr
PointCloudSource::Ptr PointCloudSourcePtr
Definition: correspondence_estimation_organized_projection.h:74
pcl::registration::CorrespondenceEstimationOrganizedProjection::clone
virtual boost::shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > clone() const
Clone and cast to CorrespondenceEstimationBase.
Definition: correspondence_estimation_organized_projection.h:183
pcl::registration::CorrespondenceEstimationOrganizedProjection::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection()
Empty constructor that sets all the intrinsic calibration to the default Kinect values.
Definition: correspondence_estimation_organized_projection.h:87
pcl::registration::CorrespondenceEstimationOrganizedProjection::ConstPtr
boost::shared_ptr< const CorrespondenceEstimationOrganizedProjection< PointSource, PointTarget, Scalar > > ConstPtr
Definition: correspondence_estimation_organized_projection.h:82
pcl::registration::CorrespondenceEstimationOrganizedProjection
CorrespondenceEstimationOrganizedProjection computes correspondences by projecting the source point c...
Definition: correspondence_estimation_organized_projection.h:61
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:137
pcl::registration::CorrespondenceEstimationOrganizedProjection::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition: correspondence_estimation_organized_projection.h:73
pcl::registration::CorrespondenceEstimationBase
Abstract CorrespondenceEstimationBase class.
Definition: correspondence_estimation.h:63