487 lines
17 KiB
C
487 lines
17 KiB
C
|
/*
|
||
|
|
* Software License Agreement (BSD License)
|
||
|
|
*
|
||
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
||
|
|
* Copyright (C) 2011, The Autonomous Systems Lab (ASL), ETH Zurich,
|
||
|
|
* Stefan Leutenegger, Simon Lynen and Margarita Chli.
|
||
|
|
* 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/common/point_tests.h> // for pcl::isFinite
|
||
|
|
#include <pcl/keypoints/agast_2d.h>
|
||
|
|
|
||
|
|
|
||
|
|
namespace pcl
|
||
|
|
{
|
||
|
|
/** \brief Detects BRISK interest points based on the original code and paper
|
||
|
|
* reference by
|
||
|
|
*
|
||
|
|
* \par
|
||
|
|
* Stefan Leutenegger,Margarita Chli and Roland Siegwart,
|
||
|
|
* BRISK: Binary Robust Invariant Scalable Keypoints,
|
||
|
|
* in Proceedings of the IEEE International Conference on Computer Vision (ICCV2011).
|
||
|
|
*
|
||
|
|
* Code example:
|
||
|
|
*
|
||
|
|
* \code
|
||
|
|
* pcl::PointCloud<pcl::PointXYZRGBA> cloud;
|
||
|
|
* pcl::BriskKeypoint2D<pcl::PointXYZRGBA> brisk;
|
||
|
|
* brisk.setThreshold (60);
|
||
|
|
* brisk.setOctaves (4);
|
||
|
|
* brisk.setInputCloud (cloud);
|
||
|
|
*
|
||
|
|
* PointCloud<pcl::PointWithScale> keypoints;
|
||
|
|
* brisk.compute (keypoints);
|
||
|
|
* \endcode
|
||
|
|
*
|
||
|
|
* \author Radu B. Rusu, Stefan Holzer
|
||
|
|
* \ingroup keypoints
|
||
|
|
*/
|
||
|
|
template <typename PointInT, typename PointOutT = pcl::PointWithScale, typename IntensityT = pcl::common::IntensityFieldAccessor<PointInT> >
|
||
|
|
class BriskKeypoint2D: public Keypoint<PointInT, PointOutT>
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
using Ptr = shared_ptr<BriskKeypoint2D<PointInT, PointOutT, IntensityT> >;
|
||
|
|
using ConstPtr = shared_ptr<const BriskKeypoint2D<PointInT, PointOutT, IntensityT> >;
|
||
|
|
|
||
|
|
using PointCloudIn = typename Keypoint<PointInT, PointOutT>::PointCloudIn;
|
||
|
|
using PointCloudOut = typename Keypoint<PointInT, PointOutT>::PointCloudOut;
|
||
|
|
using KdTree = typename Keypoint<PointInT, PointOutT>::KdTree;
|
||
|
|
using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
|
||
|
|
|
||
|
|
using Keypoint<PointInT, PointOutT>::name_;
|
||
|
|
using Keypoint<PointInT, PointOutT>::input_;
|
||
|
|
using Keypoint<PointInT, PointOutT>::indices_;
|
||
|
|
using Keypoint<PointInT, PointOutT>::k_;
|
||
|
|
|
||
|
|
/** \brief Constructor */
|
||
|
|
BriskKeypoint2D (int octaves = 4, int threshold = 60)
|
||
|
|
: threshold_ (threshold)
|
||
|
|
, octaves_ (octaves)
|
||
|
|
, remove_invalid_3D_keypoints_ (false)
|
||
|
|
{
|
||
|
|
k_ = 1;
|
||
|
|
name_ = "BriskKeypoint2D";
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Destructor. */
|
||
|
|
~BriskKeypoint2D ()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Sets the threshold for corner detection.
|
||
|
|
* \param[in] threshold the threshold used for corner detection.
|
||
|
|
*/
|
||
|
|
inline void
|
||
|
|
setThreshold (const int threshold)
|
||
|
|
{
|
||
|
|
threshold_ = threshold;
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the threshold for corner detection, as set by the user. */
|
||
|
|
inline std::size_t
|
||
|
|
getThreshold ()
|
||
|
|
{
|
||
|
|
return (threshold_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Set the number of octaves to use
|
||
|
|
* \param[in] octaves the number of octaves to use
|
||
|
|
*/
|
||
|
|
inline void
|
||
|
|
setOctaves (const int octaves)
|
||
|
|
{
|
||
|
|
octaves_ = octaves;
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Returns the number of octaves used. */
|
||
|
|
inline int
|
||
|
|
getOctaves ()
|
||
|
|
{
|
||
|
|
return (octaves_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Specify whether we should do a 2nd pass through the list of keypoints
|
||
|
|
* found, and remove the ones that do not have a valid 3D (x-y-z) position
|
||
|
|
* (i.e., are NaN or Inf).
|
||
|
|
* \param[in] remove set to true whether we want the invalid 3D keypoints removed
|
||
|
|
*/
|
||
|
|
inline void
|
||
|
|
setRemoveInvalid3DKeypoints (bool remove)
|
||
|
|
{
|
||
|
|
remove_invalid_3D_keypoints_ = remove;
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Specify whether the keypoints that do not have a valid 3D position are
|
||
|
|
* kept (false) or removed (true).
|
||
|
|
*/
|
||
|
|
inline bool
|
||
|
|
getRemoveInvalid3DKeypoints ()
|
||
|
|
{
|
||
|
|
return (remove_invalid_3D_keypoints_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////////////////////////////
|
||
|
|
inline void
|
||
|
|
bilinearInterpolation (const PointCloudInConstPtr &cloud,
|
||
|
|
float x, float y,
|
||
|
|
PointOutT &pt)
|
||
|
|
{
|
||
|
|
int u = int (x);
|
||
|
|
int v = int (y);
|
||
|
|
|
||
|
|
pt.x = pt.y = pt.z = 0;
|
||
|
|
|
||
|
|
const PointInT &p1 = (*cloud)(u, v);
|
||
|
|
const PointInT &p2 = (*cloud)(u+1, v);
|
||
|
|
const PointInT &p3 = (*cloud)(u, v+1);
|
||
|
|
const PointInT &p4 = (*cloud)(u+1, v+1);
|
||
|
|
|
||
|
|
float fx = x - float (u), fy = y - float (v);
|
||
|
|
float fx1 = 1.0f - fx, fy1 = 1.0f - fy;
|
||
|
|
|
||
|
|
float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy;
|
||
|
|
float weight = 0;
|
||
|
|
|
||
|
|
if (pcl::isFinite (p1))
|
||
|
|
{
|
||
|
|
pt.x += p1.x * w1;
|
||
|
|
pt.y += p1.y * w1;
|
||
|
|
pt.z += p1.z * w1;
|
||
|
|
weight += w1;
|
||
|
|
}
|
||
|
|
if (pcl::isFinite (p2))
|
||
|
|
{
|
||
|
|
pt.x += p2.x * w2;
|
||
|
|
pt.y += p2.y * w2;
|
||
|
|
pt.z += p2.z * w2;
|
||
|
|
weight += w2;
|
||
|
|
}
|
||
|
|
if (pcl::isFinite (p3))
|
||
|
|
{
|
||
|
|
pt.x += p3.x * w3;
|
||
|
|
pt.y += p3.y * w3;
|
||
|
|
pt.z += p3.z * w3;
|
||
|
|
weight += w3;
|
||
|
|
}
|
||
|
|
if (pcl::isFinite (p4))
|
||
|
|
{
|
||
|
|
pt.x += p4.x * w4;
|
||
|
|
pt.y += p4.y * w4;
|
||
|
|
pt.z += p4.z * w4;
|
||
|
|
weight += w4;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (weight == 0)
|
||
|
|
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
|
||
|
|
else
|
||
|
|
{
|
||
|
|
weight = 1.0f / weight;
|
||
|
|
pt.x *= weight; pt.y *= weight; pt.z *= weight;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
/** \brief Initializes everything and checks whether input data is fine. */
|
||
|
|
bool
|
||
|
|
initCompute () override;
|
||
|
|
|
||
|
|
/** \brief Detects the keypoints. */
|
||
|
|
void
|
||
|
|
detectKeypoints (PointCloudOut &output) override;
|
||
|
|
|
||
|
|
private:
|
||
|
|
/** \brief Intensity field accessor. */
|
||
|
|
IntensityT intensity_;
|
||
|
|
|
||
|
|
/** \brief Threshold for corner detection. */
|
||
|
|
int threshold_;
|
||
|
|
|
||
|
|
int octaves_;
|
||
|
|
|
||
|
|
/** \brief Specify whether the keypoints that do not have a valid 3D position are
|
||
|
|
* kept (false) or removed (true).
|
||
|
|
*/
|
||
|
|
bool remove_invalid_3D_keypoints_;
|
||
|
|
};
|
||
|
|
|
||
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
namespace keypoints
|
||
|
|
{
|
||
|
|
namespace brisk
|
||
|
|
{
|
||
|
|
/** \brief A layer in the BRISK detector pyramid. */
|
||
|
|
class PCL_EXPORTS Layer
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
// constructor arguments
|
||
|
|
struct CommonParams
|
||
|
|
{
|
||
|
|
static const int HALFSAMPLE;
|
||
|
|
static const int TWOTHIRDSAMPLE;
|
||
|
|
};
|
||
|
|
|
||
|
|
/** \brief Constructor.
|
||
|
|
* \param[in] img input image
|
||
|
|
* \param[in] width image width
|
||
|
|
* \param[in] height image height
|
||
|
|
* \param[in] scale scale
|
||
|
|
* \param[in] offset offset
|
||
|
|
*/
|
||
|
|
Layer (const std::vector<unsigned char>& img,
|
||
|
|
int width, int height,
|
||
|
|
float scale = 1.0f, float offset = 0.0f);
|
||
|
|
|
||
|
|
/** \brief Copy constructor for deriving a layer.
|
||
|
|
* \param[in] layer layer to derive from
|
||
|
|
* \param[in] mode deriving mode
|
||
|
|
*/
|
||
|
|
Layer (const Layer& layer, int mode);
|
||
|
|
|
||
|
|
/** \brief AGAST keypoints without non-max suppression.
|
||
|
|
* \param[in] threshold the keypoints threshold
|
||
|
|
* \param[out] keypoints the AGAST keypoints
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
getAgastPoints (std::uint8_t threshold, std::vector<pcl::PointUV, Eigen::aligned_allocator<pcl::PointUV> > &keypoints);
|
||
|
|
|
||
|
|
// get scores - attention, this is in layer coordinates, not scale=1 coordinates!
|
||
|
|
/** \brief Get the AGAST keypoint score for a given pixel using a threshold
|
||
|
|
* \param[in] x the U coordinate of the pixel
|
||
|
|
* \param[in] y the V coordinate of the pixel
|
||
|
|
* \param[in] threshold the threshold to use for cutting the response
|
||
|
|
*/
|
||
|
|
std::uint8_t
|
||
|
|
getAgastScore (int x, int y, std::uint8_t threshold);
|
||
|
|
/** \brief Get the AGAST keypoint score for a given pixel using a threshold
|
||
|
|
* \param[in] x the U coordinate of the pixel
|
||
|
|
* \param[in] y the V coordinate of the pixel
|
||
|
|
* \param[in] threshold the threshold to use for cutting the response
|
||
|
|
*/
|
||
|
|
std::uint8_t
|
||
|
|
getAgastScore_5_8 (int x, int y, std::uint8_t threshold);
|
||
|
|
/** \brief Get the AGAST keypoint score for a given pixel using a threshold
|
||
|
|
* \param[in] xf the X coordinate of the pixel
|
||
|
|
* \param[in] yf the Y coordinate of the pixel
|
||
|
|
* \param[in] threshold the threshold to use for cutting the response
|
||
|
|
* \param[in] scale the scale
|
||
|
|
*/
|
||
|
|
std::uint8_t
|
||
|
|
getAgastScore (float xf, float yf, std::uint8_t threshold, float scale = 1.0f);
|
||
|
|
|
||
|
|
/** \brief Access gray values (smoothed/interpolated)
|
||
|
|
* \param[in] mat the image
|
||
|
|
* \param[in] width the image width
|
||
|
|
* \param[in] height the image height
|
||
|
|
* \param[in] xf the x coordinate
|
||
|
|
* \param[in] yf the y coordinate
|
||
|
|
* \param[in] scale the scale
|
||
|
|
*/
|
||
|
|
std::uint8_t
|
||
|
|
getValue (const std::vector<unsigned char>& mat,
|
||
|
|
int width, int height, float xf, float yf, float scale);
|
||
|
|
|
||
|
|
/** \brief Get the image used. */
|
||
|
|
const std::vector<unsigned char>&
|
||
|
|
getImage () const
|
||
|
|
{
|
||
|
|
return (img_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the width of the image used. */
|
||
|
|
int
|
||
|
|
getImageWidth () const
|
||
|
|
{
|
||
|
|
return (img_width_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the height of the image used. */
|
||
|
|
int
|
||
|
|
getImageHeight () const
|
||
|
|
{
|
||
|
|
return (img_height_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the scale used. */
|
||
|
|
float
|
||
|
|
getScale () const
|
||
|
|
{
|
||
|
|
return (scale_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the offset used. */
|
||
|
|
inline float
|
||
|
|
getOffset () const
|
||
|
|
{
|
||
|
|
return (offset_);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Get the scores obtained. */
|
||
|
|
inline const std::vector<unsigned char>&
|
||
|
|
getScores () const
|
||
|
|
{
|
||
|
|
return (scores_);
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
// half sampling
|
||
|
|
inline void
|
||
|
|
halfsample (const std::vector<unsigned char>& srcimg,
|
||
|
|
int srcwidth, int srcheight,
|
||
|
|
std::vector<unsigned char>& dstimg,
|
||
|
|
int dstwidth, int dstheight);
|
||
|
|
|
||
|
|
// two third sampling
|
||
|
|
inline void
|
||
|
|
twothirdsample (const std::vector<unsigned char>& srcimg,
|
||
|
|
int srcwidth, int srcheight,
|
||
|
|
std::vector<unsigned char>& dstimg,
|
||
|
|
int dstwidth, int dstheight);
|
||
|
|
|
||
|
|
/** the image */
|
||
|
|
std::vector<unsigned char> img_;
|
||
|
|
int img_width_;
|
||
|
|
int img_height_;
|
||
|
|
|
||
|
|
/** its Fast scores */
|
||
|
|
std::vector<unsigned char> scores_;
|
||
|
|
|
||
|
|
/** coordinate transformation */
|
||
|
|
float scale_;
|
||
|
|
float offset_;
|
||
|
|
|
||
|
|
/** agast */
|
||
|
|
pcl::keypoints::agast::OastDetector9_16::Ptr oast_detector_;
|
||
|
|
pcl::keypoints::agast::AgastDetector5_8::Ptr agast_detector_5_8_;
|
||
|
|
};
|
||
|
|
|
||
|
|
/** BRISK Scale Space helper. */
|
||
|
|
class PCL_EXPORTS ScaleSpace
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
/** \brief Constructor. Specify the number of octaves.
|
||
|
|
* \param[in] octaves the number of octaves (default: 3)
|
||
|
|
*/
|
||
|
|
ScaleSpace (int octaves = 3);
|
||
|
|
~ScaleSpace ();
|
||
|
|
|
||
|
|
/** \brief Construct the image pyramids.
|
||
|
|
* \param[in] image the image to construct pyramids for
|
||
|
|
* \param[in] width the image width
|
||
|
|
* \param[in] height the image height
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
constructPyramid (const std::vector<unsigned char>& image,
|
||
|
|
int width, int height);
|
||
|
|
|
||
|
|
/** \brief Get the keypoints for the associated image and threshold.
|
||
|
|
* \param[in] threshold the threshold for the keypoints
|
||
|
|
* \param[out] keypoints the resultant list of keypoints
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
getKeypoints (const int threshold,
|
||
|
|
std::vector<pcl::PointWithScale, Eigen::aligned_allocator<pcl::PointWithScale> > &keypoints);
|
||
|
|
|
||
|
|
protected:
|
||
|
|
/** Nonmax suppression. */
|
||
|
|
inline bool
|
||
|
|
isMax2D (const std::uint8_t layer, const int x_layer, const int y_layer);
|
||
|
|
|
||
|
|
/** 1D (scale axis) refinement: around octave */
|
||
|
|
inline float
|
||
|
|
refine1D (const float s_05, const float s0, const float s05, float& max);
|
||
|
|
|
||
|
|
/** 1D (scale axis) refinement: around intra */
|
||
|
|
inline float
|
||
|
|
refine1D_1 (const float s_05, const float s0, const float s05, float& max);
|
||
|
|
|
||
|
|
/** 1D (scale axis) refinement: around octave 0 only */
|
||
|
|
inline float
|
||
|
|
refine1D_2 (const float s_05, const float s0, const float s05, float& max);
|
||
|
|
|
||
|
|
/** 2D maximum refinement */
|
||
|
|
inline float
|
||
|
|
subpixel2D (const int s_0_0, const int s_0_1, const int s_0_2,
|
||
|
|
const int s_1_0, const int s_1_1, const int s_1_2,
|
||
|
|
const int s_2_0, const int s_2_1, const int s_2_2,
|
||
|
|
float& delta_x, float& delta_y);
|
||
|
|
|
||
|
|
/** 3D maximum refinement centered around (x_layer,y_layer) */
|
||
|
|
inline float
|
||
|
|
refine3D (const std::uint8_t layer,
|
||
|
|
const int x_layer, const int y_layer,
|
||
|
|
float& x, float& y, float& scale, bool& ismax);
|
||
|
|
|
||
|
|
/** interpolated score access with recalculation when needed */
|
||
|
|
inline int
|
||
|
|
getScoreAbove (const std::uint8_t layer, const int x_layer, const int y_layer);
|
||
|
|
|
||
|
|
inline int
|
||
|
|
getScoreBelow (const std::uint8_t layer, const int x_layer, const int y_layer);
|
||
|
|
|
||
|
|
/** return the maximum of score patches above or below */
|
||
|
|
inline float
|
||
|
|
getScoreMaxAbove (const std::uint8_t layer,
|
||
|
|
const int x_layer, const int y_layer,
|
||
|
|
const int threshold, bool& ismax,
|
||
|
|
float& dx, float& dy);
|
||
|
|
|
||
|
|
inline float
|
||
|
|
getScoreMaxBelow (const std::uint8_t layer,
|
||
|
|
const int x_layer, const int y_layer,
|
||
|
|
const int threshold, bool& ismax,
|
||
|
|
float& dx, float& dy);
|
||
|
|
|
||
|
|
// the image pyramids
|
||
|
|
std::uint8_t layers_;
|
||
|
|
std::vector<pcl::keypoints::brisk::Layer> pyramid_;
|
||
|
|
|
||
|
|
// Agast
|
||
|
|
std::uint8_t threshold_;
|
||
|
|
std::uint8_t safe_threshold_;
|
||
|
|
|
||
|
|
// some constant parameters
|
||
|
|
float safety_factor_;
|
||
|
|
float basic_size_;
|
||
|
|
};
|
||
|
|
} // namespace brisk
|
||
|
|
} // namespace keypoints
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#include <pcl/keypoints/impl/brisk_2d.hpp>
|