如果不使用PCL库，该怎么写

如果不使用PCL库，我们需要手动实现点云读取和最近邻搜索的功能。下面是一个不使用PCL库的点云配准程序的C++实现，同样采用了ICP算法：

#include <iostream>
#include <fstream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

const int MAX_ITERATIONS = 50;
const double THRESHOLD = 1e-8;

struct Point3D
{
    double x, y, z;
};

// 读取点云文件
void readPointCloud(const char* filename, vector<Point3D>& pointCloud)
{
    ifstream fin(filename);
    if(!fin)
    {
        cerr << "Failed to open file: " << filename << endl;
        return;
    }

    pointCloud.clear();
    while(!fin.eof())
    {
        Point3D point;
        fin >> point.x >> point.y >> point.z;
        if(!fin.eof())
        {
            pointCloud.push_back(point);
        }
    }
    fin.close();
}

// 计算最近邻点
void nearestNeighborSearch(const vector<Point3D>& sourceCloud, const vector<Point3D>& targetCloud, vector<int>& indices, vector<double>& sqrDistances)
{
    int n = sourceCloud.size();
    indices.resize(n);
    sqrDistances.resize(n);

    for(int i=0; i<n; ++i)
    {
        double minDist = numeric_limits<double>::max();
        int minIndex = -1;
        for(int j=0; j<targetCloud.size(); ++j)
        {
            double sqrDist = pow(sourceCloud[i].x - targetCloud[j].x, 2) + pow(sourceCloud[i].y - targetCloud[j].y, 2) + pow(sourceCloud[i].z - targetCloud[j].z, 2);
            if(sqrDist < minDist)
            {
                minDist = sqrDist;
                minIndex = j;
            }
        }

        indices[i] = minIndex;
        sqrDistances[i] = minDist;
    }
}

// 点云配准函数
void pointCloudRegistration(const vector<Point3D>& sourceCloud, const vector<Point3D>& targetCloud, Matrix4d& transform)
{
    int n = sourceCloud.size();
    MatrixXd sourceMat(3, n), targetMat(3, n);
    for(int i=0; i<n; ++i)
    {
        sourceMat(0, i) = sourceCloud[i].x;
        sourceMat(1, i) = sourceCloud[i].y;
        sourceMat(2, i) = sourceCloud[i].z;

        targetMat(0, i) = targetCloud[i].x;
        targetMat(1, i) = targetCloud[i].y;
        targetMat(2, i) = targetCloud[i].z;
    }

    // 初始化变换矩阵
    transform = Matrix4d::Identity();

    for(int iter=0; iter<MAX_ITERATIONS; ++iter)
    {
        // 计算最近邻点
        MatrixXd sourceMatTransformed = transform * sourceMat;
        vector<int> indices(n);
        vector<double> sqrDistances(n);
        nearestNeighborSearch(sourceCloud, targetCloud, indices, sqrDistances);

        // 计算变换矩阵
        MatrixXd A(3, 3);
        VectorXd b(3);
        A.setZero();
        b.setZero();
        double mse = 0.0;
        for(int i=0; i<n; ++i)
        {
            if(sqrDistances[i] < THRESHOLD)
            {
                Vector3d sourcePoint(sourceMat(0, i), sourceMat(1, i), sourceMat(2, i));
                Vector3d targetPoint(targetMat(0, indices[i]), targetMat(1, indices[i]), targetMat(2, indices[i]));
                mse += (sourcePoint - targetPoint).squaredNorm();

                A += sourcePoint * targetPoint.transpose();
                b += targetPoint;
            }
        }

        if(mse/n < THRESHOLD)
        {
            break;
        }

        Vector3d t = A.colPivHouseholderQr().solve(b);
        Matrix4d T = Matrix4d::Identity();
        T.block<3, 1>(0, 3) = t;
        transform = T * transform;
    }
}

int main(int argc, char** argv)
{
    if(argc < 3)
    {
        cerr << "Usage: " << argv[0] << " source.ply target.ply" << endl;
        return -1;
    }

    vector<Point3D> sourceCloud, targetCloud;
    readPointCloud(argv[1], sourceCloud);
    readPointCloud(argv[2], targetCloud);

    Matrix4d transform;
    pointCloudRegistration(sourceCloud, targetCloud, transform);
    cout << "Transform Matrix:" << endl << transform << endl;

    return 0;
}

这个程序手动实现了读取点云文件和最近邻搜索的功能，其余部分与使用PCL库的点云配准程序实现相同。需要注意的是，手动实现最近邻搜索会比使用PCL库的实现慢很多，如果需要处理的点云较大，应该使用PCL库。