ICP算法的C++代码

当然，下面是一个示例的ICP算法的C代码：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define MAX_POINTS 100

typedef struct {
    double x;
    double y;
} Point;

void icp(Point *source, Point *target, int numPoints, double *dx, double *dy, double *theta) {
    int i;
    
    // 初始化dx, dy, theta为0
    *dx = 0.0;
    *dy = 0.0;
    *theta = 0.0;

    // 计算源点集和目标点集的质心
    Point sourceCentroid = {0.0, 0.0};
    Point targetCentroid = {0.0, 0.0};

    for (i = 0; i < numPoints; i++) {
        sourceCentroid.x += source[i].x;
        sourceCentroid.y += source[i].y;

        targetCentroid.x += target[i].x;
        targetCentroid.y += target[i].y;
    }

    sourceCentroid.x /= numPoints;
    sourceCentroid.y /= numPoints;

    targetCentroid.x /= numPoints;
    targetCentroid.y /= numPoints;

    // 计算源点集和目标点集的协方差矩阵
    double cov00 = 0.0;
    double cov01 = 0.0;
    double cov10 = 0.0;
    double cov11 = 0.0;

    for (i = 0; i < numPoints; i++) {
        cov00 += (source[i].x - sourceCentroid.x) * (target[i].x - targetCentroid.x);
        cov01 += (source[i].x - sourceCentroid.x) * (target[i].y - targetCentroid.y);
        cov10 += (source[i].y - sourceCentroid.y) * (target[i].x - targetCentroid.x);
        cov11 += (source[i].y - sourceCentroid.y) * (target[i].y - targetCentroid.y);
    }

    // 计算旋转角度和平移量
    double det = cov00 * cov11 - cov01 * cov10;
    *theta = atan2(cov01 + cov10, cov00 - cov11) / 2;
    *dx = targetCentroid.x - cos(*theta) * sourceCentroid.x + sin(*theta) * sourceCentroid.y;
    *dy = targetCentroid.y - sin(*theta) * sourceCentroid.x - cos(*theta) * sourceCentroid.y;
}

int main() {
    // 定义源点集和目标点集
    Point source[MAX_POINTS];
    Point target[MAX_POINTS];
    
    int numPoints;
    printf("请输入点的数量：");
    scanf("%d", &numPoints);
    
    // 读取源点集和目标点集的坐标
    printf("请输入源点集的坐标(x y)：\n");
    for (int i = 0; i < numPoints; i++) {
        scanf("%lf %lf", &source[i].x, &source[i].y);
    }
    
    printf("请输入目标点集的坐标(x y)：\n");
    for (int i = 0; i < numPoints; i++) {
        scanf("%lf %lf", &target[i].x, &target[i].y);
    }

    // 执行ICP算法
    double dx, dy, theta;
    icp(source, target, numPoints, &dx, &dy, &theta);
    
    // 输出结果
    printf("平移量(dx, dy) = (%lf, %lf)\n", dx, dy);
    printf("旋转角度(theta) = %lf\n", theta);

    return 0;
}

这段代码实现了一个简单的ICP算法，它通过计算源点集和目标点集之间的平移量和旋转角度来实现点云配准。你可以根据自己的需求进行修改和扩展。希望对你有帮助！