最近好像跟圆柱杠上了，思考的问题都是和圆柱有关系的……在用类似于RANSAC的方法从点云里提取出圆柱之后，最好对提取出的点重新进行拟合（refitting）来让结果更加准确。可以采用非线性最小二乘的方法来进行拟合，比如Levenberg - Marquardt（LM）法。上学期选修的物理动画仿真课里似乎讲过相关的内容，不过当时没有认真听，只能自己再研究一遍了。
1. 背景
三维空间中点到直线的距离：假设直线的单位方向向量为 $\overrightarrow{l}$，直线上一点为 $B$，那么空间中一点 $A$ 到直线的距离 $d$ 可以通过叉乘来计算：
\[d = |\overrightarrow{l} \times \overrightarrow{BA}|\]向量叉乘的导数是：
\[\frac{d}{dt}(\overrightarrow{x} \times \overrightarrow{y}) = \dot{\overrightarrow{x}} \times \overrightarrow{y} + \overrightarrow{x} \times \dot{\overrightarrow{y}}\]2. 参数设置
将圆柱拟合问题转化为一个最小二乘优化的问题。我们需要求得的圆柱参数有：圆柱轴线的单位方向 $\overrightarrow{A}$，轴线上一点的坐标 $x$，以及圆柱的半径 $r$。 用于拟合的点集为$P$，点集里任意一点 $x_i$ 到圆柱表面的距离为:
\[d_i =|f_i - r|\]其中 $f_i$ 即为 $x_i$ 到圆柱轴线的距离，可以用1里的方法来求。 拟合的目标是最小化所有点到圆柱表面的距离，所以目标函数为：
\[F(x,\overrightarrow{A},r) = \sum(f_i-r)^2 = \sum[|(x_i-x) \times \overrightarrow{A}| - r]^2\]3. LM算法的代码实现
LM算法的原理在此就不赘述了（网上很多博客里都写得很清楚）。我借助了Eigen自带的函数来实现圆柱的拟合，只需要自己定义目标函数和Jacobi矩阵就可以了。此外，也可以用Eigen::NumericalDiff 来计算Jacobi矩阵（效果应该不如自己定义来得好），用这个类需要#include <unsupported/Eigen/NumericalDiff>
。
目标函数和Jacobi矩阵的计算参照了1和2的内容，以及 Least-Squares Fitting Algorithms of the NIST Algorithm Testing System 这篇文章，它定义了很多基本形状的最小二乘拟合方法（比如平面，球形，圆柱，圆锥等等）。
LM算法部分的代码如下，头文件里需要#include <unsupported/Eigen/NonLinearOptimization>。其中，x存储了最终输出的圆柱体参数，按照：轴线上一点（pt_on_axis），轴线的单位方向向量（axis），圆柱的半径这个顺序存储。mVec储存了用来拟合的点。
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
typedef _Scalar Scalar;
enum {
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
int m_inputs, m_values;
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
int inputs() const { return m_inputs; }
int values() const { return m_values; }
};
struct my_functor : Functor<float>
{
private:
Eigen::MatrixXf mVec;
int mLen;
public:
void setValues(Eigen::MatrixXf vec)
{
mVec = vec;
mLen = vec.cols();
}
my_functor(void) : Functor<float>(7, 500) {}
my_functor(int src, int dst) : Functor<float>(src, dst) {}
//params in x: coordinates of a point on axis, axis direction(unit),radius
//objective function
int operator()(const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
{
Eigen::Vector3f axis(x(3), x(4), x(5));
Eigen::Vector3f pt_on_axis(x(0), x(1), x(2));
for (int i = 0; i < mLen; i++)
{
Eigen::Vector3f pt = mVec.col(i);
//fi:distance from pt to the cylinder axis
float fi = (axis.cross(pt - pt_on_axis)).norm();
fvec(i) = fi - x(6);
}
return 0;
}
//jacobian matrix
int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
{
Eigen::Vector3f axis(x(3), x(4), x(5));
Eigen::Vector3f pt_on_axis(x(0), x(1), x(2));
for (int i = 0; i < mLen; i++)
{
//get the vector of current point
Eigen::Vector3f pt = mVec.col(i);
float gi = axis.dot(pt - pt_on_axis);
//fi:distance from pt to the cylinder axis
float fi = (axis.cross(pt - pt_on_axis)).norm();
if (fi != 0.0f)
{
//for each param in x
//the point on axis
fjac(i, 0) = (axis(0)*gi - (pt(0) - pt_on_axis(0))) / fi;
fjac(i, 1) = (axis(1)*gi - (pt(1) - pt_on_axis(1))) / fi;
fjac(i, 2) = (axis(2)*gi - (pt(2) - pt_on_axis(2))) / fi;
//axis
fjac(i, 3) = gi * fjac(i, 0);
fjac(i, 4) = gi * fjac(i, 1);
fjac(i, 5) = gi * fjac(i, 2);
//radius
fjac(i, 6) = -1;
}
else
{
Eigen::Vector3f gradient1(sqrt(1 - axis(0)*axis(0)), sqrt(1 - axis(1)*axis(1)), sqrt(1 - axis(2)*axis(2)));
Eigen::VectorXf gradient(7);
gradient << gradient1, gi*gradient1, -1;
fjac.row(i) = gradient.transpose();
}
}
return 0;
}
};
以下是应用部分的代码，其中Cylinder是我自己写的类，里面存储了初始的轴线方向，轴线上的点坐标，半径以及待拟合的点（PCL的点云形式）。由于初始的参数已经很接近最优参数了，迭代次数可以设置得小一些。
//refitting using LM algorithm
void refitting_cylinder(Cylinder& c)
{
//get candidate points
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
c.get_cloud(cloud);
Eigen::MatrixXf input_points(3, cloud->size());
//get the input data
for (int i = 0; i < cloud->size(); i++)
{
input_points.col(i) = cloud->points[i].getVector3fMap();
}
//get the initial params before iteration
Eigen::VectorXf x(7);
Eigen::Vector3f axis0, center0;
c.get_axis(axis0); //initial axis
axis0 = axis0 / axis0.norm();
c.get_center(center0); //center is a point on the axis
float radius0 = c.get_radius(); //initial raidus
x << center0, axis0, radius0;
cout << "initial x" << x.transpose() << endl;
//LM algorithm
my_functor functor(7,cloud->size());
functor.setValues(input_points);
Eigen::LevenbergMarquardt<my_functor, float> lm(functor);
lm.parameters.maxfev = 500;
lm.parameters.xtol = 1.0e-10;
int ret = lm.minimize(x);
c.reset_params(x); //reset the params of c
cout << "x that minimizes the function: " << x.transpose() << endl;
return;
}