描述
使用C++代码来实现二维点云ICP算法
二维点云ICP原理推导
特点说明:
- ICP算法中的loss计算方式,可以根据自己实际需要来调整。我这里使用的是,目标点云A中的某个点 a,从源点云 B 中找到距离点 a 最近的点 b,总的loss就是这些a-b距离之和
- 正常的loss计算方式,应该是从源点云B中的每个点 b,去目标点云A里寻找最近点
- loss的定义方式一定要好好思考,结合自己的实际需要,不要只会把我下面的代码搬过去,因为可能不work
代码
这里我只提供一个ICP的api接口,是从我的一段代码中扒下来的,有一些修改是在CSDN中直接修改的,没有编译试过。
不过我应该是不会出错的,有问题可以叫我
#include <numeric>
// algorithm ICP
// source_x,source_y: points need to trans
// standard_x,standard_y: points are target
// trans_x,trans_y: points that are source points after transform
void ICP(std::vector<float> source_x, std::vector<float> source_y,
std::vector<float> standard_x, std::vector<float> standard_y,
std::vector<float> &trans_x, std::vector<float> &trans_y)
{
std::vector<float> A_x = standard_x;
std::vector<float> A_y = standard_y;
std::vector<float> B_x = source_x;
std::vector<float> B_y = source_y;
int iter_times = 0;
double loss_now;
double loss_improve = 1000;
double loss_before = getLossShapeMatch(source_x, source_y, standard_x, standard_y)/standard_x.size();
double A_x_sum = std::accumulate(std::begin(A_x), std::end(A_x), 0.0);
double A_x_mean = A_x_sum / A_x.size();
double A_y_sum = std::accumulate(std::begin(A_y), std::end(A_y), 0.0);
double A_y_mean = A_y_sum / A_y.size();
std::vector<float> A_x_;
std::vector<float> A_y_;
A_x_.resize(A_x.size());
A_y_.resize(A_x.size());
for (int i = 0 ; i < A_x.size(); i++)
{
A_x_[i] = A_x[i] - A_x_mean;
A_y_[i] = A_y[i] - A_y_mean;
}
while(iter_times < 20 && loss_improve > 0.0005)
{
double B_x_sum = std::accumulate(std::begin(B_x), std::end(B_x), 0.0);
double B_x_mean = B_x_sum / B_x.size();
double B_y_sum = std::accumulate(std::begin(B_y), std::end(B_y), 0.0);
double B_y_mean = B_y_sum / B_y.size();
std::vector<float> B_x_;
std::vector<float> B_y_;
B_x_.resize(B_x.size());
B_y_.resize(B_x.size());
for (int i = 0 ; i < B_x.size(); i++)
{
B_x_[i] = B_x[i] - B_x_mean;
B_y_[i] = B_y[i] - B_y_mean;
}
double w_up = 0;
double w_down = 0;
for (int i = 0 ; i < A_x_.size(); i++)
{
int id = getClosestPointID(A_x_[i], A_y_[i], B_x_, B_y_);
double w_up_i = A_x_[i]*B_y_[id] - A_y_[i]*B_x_[id];
double w_down_i = A_x_[i]*B_x_[id] + A_y_[i]*B_y_[id];
w_up = w_up + w_up_i;
w_down = w_down + w_down_i;
}
double theta = atan2(w_up, w_down);
double x = A_x_mean - cos(theta)*B_x_mean - sin(theta)*B_y_mean;
double y = A_y_mean + sin(theta)*B_x_mean - cos(theta)*B_y_mean;
for (int i = 0 ; i < B_x.size(); i++)
{
double xx = B_x[i];
double yy = B_y[i];
B_x[i] = cos(theta)*xx + sin(theta)*yy + x;
B_y[i] = -sin(theta)*xx + cos(theta)*yy + y;
}
iter_times++;
double loss_now = getLossShapeMatch(B_x, B_y, A_x, A_y,)/A_x.size();
loss_improve = loss_before - loss_now;
// std::cout<<"itertime: "<<iter_times<<", loss now: "<<loss_now<<", improve: "<<loss_improve<<std::endl;
loss_before = loss_now;
}
trans_x = B_x;
trans_y = B_y;
}
// get all points distance of {[test_x, test_y]} and {[standard_x,standard_y]} as loss
double getLossShapeMatch(std::vector<float> test_x, std::vector<float> test_y, std::vector<float> standard_x, std::vector<float> standard_y)
{
double loss_sum = 0;
for (int i = 0 ; i < standard_x.size() ; i++)
{
double closest_x, closest_y;
getClosestPoint(standard_x[i], standard_y[i], test_x, test_y, closest_x, closest_y);
double loss = getDistanceOfTwoPoints(standard_x[i], standard_y[i], closest_x, closest_y);
loss_sum = loss_sum + loss;
}
return loss_sum;
}
// get closet point of [x,y] in {[sourcePoints_x, sourcePoints_y]}
void getClosestPoint(float x, float y, std::vector<float> sourcePoints_x, std::vector<float> sourcePoints_y, double& find_x, double& find_y)
{
double dist_min = 10000000;
for (int i = 0 ; i < sourcePoints_x.size(); i++)
{
double dist = getDistanceOfTwoPoints(x,y,sourcePoints_x[i],sourcePoints_y[i]);
if (dist < dist_min)
{
dist_min = dist;
find_x = sourcePoints_x[i];
find_y = sourcePoints_y[i];
}
}
}
int getClosestPointID(double x, double y, std::vector<float> sourcePoints_x, std::vector<float> sourcePoints_y)
{
int id = 0;
double dist_min = 1000000;
for (int i = 0 ; i < sourcePoints_x.size(); i++)
{
double dist = getDistanceOfTwoPoints(x, y, sourcePoints_x[i], sourcePoints_y[i]);
if (dist < dist_min)
{
dist_min = dist;
id = i;
}
}
return id;
}
// distance of two points (x1, y1) and (x2, y2)
double getDistanceOfTwoPoints(double x1, double y1, double x2, double y2)
{
return sqrt(pow(x1-x2, 2) + pow(y1-y2, 2));
}