spin_image特征描述子原理
特征描述子网上有很多博主介绍了,我列出看到的一些文章,由于时间关系没有详细阅读论文。
看了这篇文章PCL Spin Image 旋转图像,其实原理很简单,总结一下就是:以查询点的法向量(旋转轴可以指定)为旋转轴,根据指定半径r旋转一周生成圆柱,将落入圆柱的近邻点分别与查询点计算两个维度的特征,然后将所有近邻点的二维特征生成二维的spin_image特征图。
步骤为:
- 指定查询点(计算查询点的附近的spin_image),指定查询点的旋转轴和旋转半径
- 根据半径得到查询点附近的k近邻(kdtreeflann搜索)
- 计算各近邻点与查询点之间的二维特征
- 将各二维特征落入spin_image,由于分辨率的存在,需要对二维特征进行双线性插值
- 标准化,将spin_image特征矩阵拉成成一维向量,返回
上述计算二维特征的过程用下图来展示:
下面对应二维特征的计算过程:
下面是双线性插值的过程:
PCL源码
pcl代码很规范,基本每个文件都有固定的作用。
- hpp文件:各类的具体实现代码,是最核心的文件。hpp文件可以看做是cpp文件和h文件的结合,其优缺点可以网上搜索下。
- h文件:表示各类的声明
- cpp文件:大部分cpp文件都很简单,但是都有一个宏定义
PCL_INSTANTIATE_PRODUCT
用来初始化各点云类型的类模板,加速编译。
其实我们在调用pcl各函数接口时,都只需要包含h文件即可,因此可以从h文件入手查看各类的定义。
“spin_image.h”:
定义了SpinImageEstimation类,继承自Feature类,而Feature类继承自PCLBase类。
- PCLBase类定义了常用的输入点云setInputCloud和输入索引setIndices等虚函数,供子类来重载。
- Feature类则定义了最核心的compute函数以及computeFeature虚函数,用于各个子类编写特征提取算法。除此之外,还定义了searchForNeighbors成员函数,可以用于各子类调用k近邻搜索。
该类重载了initCompute函数,用来检查成员变量是否有效,并且初始化参数。重载了computeFeature函数,用来计算特征。当然computeFeature实际上是调用了computeSiForPoint函数,而computeSiForPoint函数是计算特征的主体部分。
下面详细介绍各成员函数和变量的具体用途:
SpinImageEstimation类(继承自feature类)
//成员函数
:getClassName(继承自feature类):返回特征名字feature_name_变量
构造函数
setImageWidth:设置image_width_参数,为spin-image分辨率,即一个维度有多少bins
setSupportAngle:设置support_angle_cos_参数,即输入点的法向量与surface_范围内的法向量之间的cos值,因此在0~1之间
setMinPointCountInNeighbourhood:设置min_pts_neighb_参数,指定生成spin image所需最少的点个数
setInputNormals:设置input_normals_参数,设置输入的法向量,与setInputCloud函数输入的点云相对应
setRotationAxis:设置旋转轴,输入一个法向量。use_custom_axis_为true,use_custom_axes_cloud_为false
setInputRotationAxes:设置旋转轴,use_custom_axis_为false,use_custom_axes_cloud_为true
useNormalsAsRotationAxis:使用法向量作为旋转轴,use_custom_axis_为false,use_custom_axes_cloud_为true
setAngularDomain:设置is_angular_参数
setRadialStructure:设置is_radial_参数,使用极坐标系
computeFeature(virtual):虚函数重载
initCompute(virtual):虚函数重载
computeSiForPoint:为扫描的点计算spin-image,并且返回一个矩阵
//变量
:feature_name_(继承自feature类)
:indices_(继承自feature类):该变量是要计算特征的点的索引
:search_radius_(继承自feature类)
:k_(继承自feature类)
:surface_(继承自feature类)
:fake_surface_(继承自feature类)
:input_(继承自PCLBase类)
input_normals_:输入的法向量
rotation_axes_cloud_:旋转轴向量
is_angular_:使用法向量之间的角度来计算特征而不是用点的坐标
rotation_axis_:旋转轴向量
use_custom_axis_:setRotationAxis和setInputRotationAxes函数使用
use_custom_axes_cloud_:setRotationAxis和setInputRotationAxes函数使用
is_radial_:使用极坐标系来描述spin_image而不是使用笛卡尔坐标
image_width_:分辨率,一个维度有多少bins
support_angle_cos_:输入点的法向量与surface_范围内的法向量之间的cos值
min_pts_neighb_:指定生成spin image所需最少的点个数
“spin_image.hpp”:
该文件是类的实现文件,主要是三个成员函数的实现,分别是computeFeature,initCompute,computeSiForPoint。
下面放出自己蹩脚注释版的hpp文件:
#ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_
#define PCL_FEATURES_IMPL_SPIN_IMAGE_H_
#include <limits> //得到各数据类型的极限
// cout<<"numeric_limits<unsigned short>::min()= "
//<<numeric_limits<unsigned short>::min()<<endl; //unsigned short的最小值
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/exceptions.h> //异常
#include <pcl/kdtree/kdtree_flann.h> //kdtree search
#include <pcl/features/spin_image.h>
#include <cmath> //cmath为c++math头文件 math.h为c头文件,c++这两个头文件都可以用
//
//得到归一化的spin_image,返回的是一个ArrayXXd(image_width, 2*image_width_)矩阵
template <typename PointInT, typename PointNT, typename PointOutT>
pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::SpinImageEstimation (
unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) :
input_normals_ (), rotation_axes_cloud_ (),
is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false),
is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos),
min_pts_neighb_ (min_pts_neighb)
{
assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
feature_name_ = "SpinImageEstimation";
}
//
//计算spin_image特征的函数
template <typename PointInT, typename PointNT, typename PointOutT> Eigen::ArrayXXd
pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeSiForPoint (int index) const
{
assert (image_width_ > 0);
assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
//得到指定索引的点坐标
//map映射函数,将结构体中的data数组按照Vector3f数据类型映射到origin_point变量,享用同一片内存
const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
//根据输入的法向量得到查询点的法向量
Eigen::Vector3f origin_normal;
origin_normal =
input_normals_ ?
input_normals_->points[index].getNormalVector3fMap () :
Eigen::Vector3f (); // just a placeholder; should never be used!
//得到旋转轴向量 -- 使用输入的查询点法向量
const Eigen::Vector3f rotation_axis = use_custom_axis_ ?
rotation_axis_.getNormalVector3fMap () :
use_custom_axes_cloud_ ?
rotation_axes_cloud_->points[index].getNormalVector3fMap () :
origin_normal;
//定义要返回的spin_image变量,第二个变量m_averAngles是按照极坐标系求解方法建立的变量
Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
// OK, we are interested in the points of the cylinder of height 2*r and
// base radius r, where r = m_dBinSize * in_iImageWidth
// it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
// suppose that points are uniformly distributed, so we lose ~40%
// according to the volumes ratio
double bin_size = 0.0;
if (is_radial_)
bin_size = search_radius_ / image_width_;
else
bin_size = search_radius_ / image_width_ / sqrt(2.0);
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;
//kdtree搜索,返回搜索到的点的数量
const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
if (neighb_cnt < static_cast<int> (min_pts_neighb_))
{
throw PCLException (
"Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
"spin_image.hpp", "computeSiForPoint");
}
// for all neighbor points
for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
{
// first, skip the points with distant normals
double cos_between_normals = -2.0; // should be initialized if used
//support_angle_cos_ > 0代表只选取与查询点法向量夹角小于90度的点
//如果角度超过设定的角度,那么退出本次循环
if (support_angle_cos_ > 0.0 || is_angular_) // not bogus
{
//查询点法向量和近邻点的法向量相乘-得到cos值,因为法向量长度为单位长度
cos_between_normals = origin_normal.dot (input_normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ());
//如果查询点的法向量和近邻点的法向量夹角余弦值大于1,说明法向量没有标准化,可能长度不是单位量
if (fabs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ())) // should be okay for numeric stability
{
PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n",
getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
throw PCLException ("Some normals are not normalized",
"spin_image.hpp", "computeSiForPoint");
}
//确保余弦值在-1和1之间
cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
//小于设定的cos值,说明夹角比设定的大
if (fabs (cos_between_normals) < support_angle_cos_ ) // allow counter-directed normals
{
continue;
}
//如果小于0代表其补角与查询点的法向量之间的夹角小于设定值
if (cos_between_normals < 0.0)
{
//取补角
cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now
}
}
// now compute the coordinate in cylindric coordinate system associated with the origin point
const Eigen::Vector3f direction (
surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point);
//计算方向向量的长度 - L2范数
const double direction_norm = direction.norm ();
if (fabs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())
continue; // ignore the point itself; it does not contribute really
assert (direction_norm > 0.0);
//计算查询点法向量和方向向量的夹角余弦值
// the angle between the normal vector and the direction to the point
double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
if (fabs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability
{
PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n",
getClassName ().c_str (), index, cos_dir_axis);
throw PCLException ("Some rotation axis is not normalized",
"spin_image.hpp", "computeSiForPoint");
}
cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
// compute coordinates w.r.t. the reference frame
double beta = std::numeric_limits<double>::signaling_NaN ();
double alpha = std::numeric_limits<double>::signaling_NaN ();
//可以看做是极坐标系
if (is_radial_) // radial spin image structure
{
//取两个法向量夹角的余角
beta = asin (cos_dir_axis); // yes, arc sine! to get the angle against tangent, not normal!
//用方向向量的长度作为alpha
alpha = direction_norm;
}
//这是论文中的方法,计算alpha和beta,落到二维网格上
else // rectangular spin-image structure
{
beta = direction_norm * cos_dir_axis;
alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
//如果近邻点与查询点的高度差超过image_width_,那么去掉该点,不统计进spin_image
if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
{
continue; // outside the cylinder
}
}
assert (alpha >= 0.0);
assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
//双线性插值
// bilinear interpolation
/**
* std::floor 和 std::ceil都是对变量进行四舍五入,只不过四舍五入的方向不同。
* std::floor -->向下取整数
* std::ceil -->向上取整数
*/
//计算alpha_bin和beta_bin,可以看做由距离和分辨率计算像素位置,向下取整
//这里宽度为image_width_,高度为2*image_width_,因为有可能在查询点的下方
double beta_bin_size = is_radial_ ? (M_PI / 2 / image_width_) : bin_size;
int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_);
assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
//bin_size = search_radius_ / image_width_
int alpha_bin = int(std::floor (alpha / bin_size));
assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
//边缘处理 -- 减去一个最小量
if (alpha_bin == static_cast<int> (image_width_)) // border points
{
alpha_bin--;
// HACK: to prevent a > 1
alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
}
if (beta_bin == int(2*image_width_) ) // border points
{
beta_bin--;
// HACK: to prevent b > 1
beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
}
//下面计算与两边的bin的差值
double a = alpha/bin_size - double(alpha_bin);
double b = beta/beta_bin_size - double(beta_bin-int(image_width_));
assert (0 <= a && a <= 1);
assert (0 <= b && b <= 1);
//进行双边滤波 -- 看论文
//Array定义的数组相对于Matrix类似,只是访问时需要用括号(),另外定义的数组可以直接和标量相加
//4个值相加为1
m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b);
m_matrix (alpha_bin+1, beta_bin) += a * (1-b);
m_matrix (alpha_bin, beta_bin+1) += (1-a) * b;
m_matrix (alpha_bin+1, beta_bin+1) += a * b;
//这种方法不是将点的特征生成spin_image,而是用方向向量和法向量之间的夹角
//将角度分成4份,占比相加为1
if (is_angular_)
{
m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * acos (cos_between_normals);
m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * acos (cos_between_normals);
m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * acos (cos_between_normals);
m_averAngles (alpha_bin+1, beta_bin+1) += a * b * acos (cos_between_normals);
}
}
if (is_angular_)
{
// transform sum to average
//Array除法是每个元素各自相除
//该运算可以看做是求期望
m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ()); // +eps to avoid division by zero
}
else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1
{
// normalization
m_matrix /= m_matrix.sum();
}
return m_matrix;
}
//
//initCompute初始化各种参数变量,并且检查参数是否设置正确
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::initCompute ()
{
if (!Feature<PointInT, PointOutT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
// Check if input normals are set
if (!input_normals_)
{
PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
// Check if the size of normals is the same as the size of the surface
if (input_normals_->points.size () != input_->points.size ())
{
PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
PCL_ERROR ("The number of points in the input dataset differs from ");
PCL_ERROR ("the number of points in the dataset containing the normals!\n");
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
// We need a positive definite search radius to continue
if (search_radius_ == 0)
{
PCL_ERROR ("[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
if (k_ != 0)
{
PCL_ERROR ("[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
// If the surface won't be set, make fake surface and fake surface normals
// if we wouldn't do it here, the following method would alarm that no surface normals is given
if (!surface_)
{
surface_ = input_;
fake_surface_ = true;
}
//if (fake_surface_ && !input_normals_)
// input_normals_ = normals_; // normals_ is set, as checked earlier
assert(!(use_custom_axis_ && use_custom_axes_cloud_));
if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes
&& !input_normals_)
{
PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
// Cleanup
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals
&& !input_normals_)
{
PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
// Cleanup
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
if (use_custom_axes_cloud_
&& rotation_axes_cloud_->size () == input_->size ())
{
PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
// Cleanup
Feature<PointInT, PointOutT>::deinitCompute ();
return (false);
}
return (true);
}
//
//输入要计算特征的点的索引数组,计算特征,然后赋值到输出变量的直方图变量中,便于查看
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
for (int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input)
{
//将要计算特征的点的索引indices_输入到computeSiForPoint函数中,计算特征,返回spin_iamge
Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
//按照返回的spin_iamge赋值到输出变量的直方图变量中
// Copy into the resultant cloud
for (int iRow = 0; iRow < res.rows () ; iRow++)
{
for (int iCol = 0; iCol < res.cols () ; iCol++)
{
output.points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast<float> (res (iRow, iCol));
}
}
}
}
#define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>;
#endif // PCL_FEATURES_IMPL_SPIN_IMAGE_H_
程序中特征计算流程很清晰,只不过相比于论文,PCL还添加了另一种计算特征的方法,就是用向量间的夹角来代替近邻点与查询点的坐标关系来计算特征,这种方法在论文Unsupervised Discovery of Object Classes
from Range Data using Latent Dirichlet Allocation中有描述。
“spin_image.cpp”:
本文件很简短,只有一个宏定义,初始化了指定点云数据类型的类模板。
#include <pcl/features/impl/spin_image.hpp>
#ifndef PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
// Instantiations of specific point types
//只编译一些核心的point_types
#ifdef PCL_ONLY_CORE_POINT_TYPES
PCL_INSTANTIATE_PRODUCT(SpinImageEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointNormal))((pcl::Normal)(pcl::PointNormal))((pcl::Histogram<153>)))
#else
PCL_INSTANTIATE_PRODUCT(SpinImageEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::Histogram<153>)))
#endif
#endif // PCL_NO_PRECOMPILE
PCL_INSTANTIATE_PRODUCT(SpinImageEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::Histogram<153>)))
这句是一个带参数的宏定义,三个参数分别代表输入点云数据类型、输入法向量数据类型、输出结果数据类型。其中输出结果的数据类型决定了调用哪个类的compute重载函数。
下面列出了该宏定义到底初始化了哪些数据类型的类模板:
BOOST_PP_SEQ_FOR_EACH_PRODUCT(PCL_INSTANTIATE_PRODUCT_IMPL, \
((SpinImageEstimation))((pcl::PointXYZ) (pcl::PointXYZI) (pcl::PointXYZL) \
(pcl::PointXYZRGBA) (pcl::PointXYZRGB) (pcl::PointXYZRGBL) (pcl::PointXYZHSV) \
(pcl::InterestPoint) (pcl::PointNormal) (pcl::PointXYZRGBNormal) \
(pcl::PointXYZINormal) (pcl::PointXYZLNormal) (pcl::PointWithRange) \
(pcl::PointWithViewpoint) (pcl::PointWithScale) (pcl::PointSurfel) (pcl::PointDEM))\
((pcl::Normal) (pcl::PointNormal) (pcl::PointXYZRGBNormal) (pcl::PointXYZINormal) \
(pcl::PointXYZLNormal) (pcl::PointSurfel))((pcl::Histogram<153>)))
该宏是在instantiate.hpp文件中定义的,主要作用是对输入点云类型、输入法向量类型和输出结果类型所包含的数据类型进行排列组合,完成类模板的初始化。比如,在该过程中,就包含了下面这种类型的初始化:
template class PCL_EXPORTS \
pcl::SpinImageEstimation<pcl::PointXYZ,pcl::Normal,pcl::Histogram<153>>
instantiate.hpp文件中包含了很多boost宏,我大致列出该初始化过程中使用到的宏:
/**
* BOOST_PP_SEQ_HEAD : 取出seq中第一个元素
* BOOST_PP_EXPAND : 扩展参数列表(根据调用的宏,展开宏中的所有的参数列表)
* BOOST_PP_SEQ_TAIL : 取出seq中除了头部后的剩余元素
e.g.:
#define SEQ (a)(b)(c)
BOOST_PP_SEQ_HEAD(SEQ) // expands to a
BOOST_PP_SEQ_TAIL(SEQ) // expands to (b)(c)
* BOOST_PP_SEQ_TO_TUPLE
e.g.:
BOOST_PP_SEQ_TO_TUPLE(SEQ) // expands to (a, b, c)
* BOOST_PP_CAT : 连接操作符
e.g.:
BOOST_PP_CAT(x, BOOST_PP_CAT(y, z)) // expands to xyz
*/
上面这三个文件展示了PCL中计算特征描述子的一个典型的代码结构,各文件作用非常清晰,非常便于阅读,自己的能力有限,注释得不是很清晰,推荐直接阅读。
如果你想加载一篇你写过的.md文件,在上方工具栏可以选择导入功能进行对应扩展名的文件导入,
继续你的创作。