两条三维空间轨迹对齐C++

两条三维空间轨迹对齐可以通过以下步骤实现：

1. 对两条轨迹进行采样，得到相同数量的点，可以通过等间距采样或者根据曲率进行采样。

2. 根据采样点进行轨迹的配准。可以采用最小二乘法将一个轨迹对齐到另一个轨迹，具体方法是将两条轨迹的每个点都映射到同一坐标系下，然后通过最小化两条轨迹之间的距离来优化配准参数。

3. 通过优化后的配准参数对第二条轨迹进行变换，使其与第一条轨迹重合。

以下是一个简单的C++示例代码：

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace Eigen;

// 定义点结构体
struct Point3D {
    double x, y, z;
};

// 采样函数，等间距采样
std::vector<Point3D> sample(std::vector<Point3D> traj, int num_samples) {
    std::vector<Point3D> samples;
    int step = traj.size() / num_samples;
    for (int i = 0; i < num_samples; i++) {
        samples.push_back(traj[i * step]);
    }
    return samples;
}

// 对齐函数
void align(std::vector<Point3D>& traj1, std::vector<Point3D>& traj2) {
    // 采样
    int num_samples = 100;
    std::vector<Point3D> samples1 = sample(traj1, num_samples);
    std::vector<Point3D> samples2 = sample(traj2, num_samples);

    // 构建矩阵A和向量b
    MatrixXd A(3 * num_samples, 6);
    VectorXd b(3 * num_samples);
    for (int i = 0; i < num_samples; i++) {
        Point3D pt1 = samples1[i];
        Point3D pt2 = samples2[i];
        A.block<3, 3>(3 * i, 0) = Matrix3d::Identity();
        A.block<3, 3>(3 * i, 3) = Matrix3d::Zero();
        b.segment<3>(3 * i) = Vector3d(pt1.x, pt1.y, pt1.z) - Vector3d(pt2.x, pt2.y, pt2.z);
    }

    // 最小二乘求解
    VectorXd x = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);

    // 变换轨迹2
    Matrix3d R = AngleAxisd(x(0), Vector3d::UnitX()) *
                AngleAxisd(x(1), Vector3d::UnitY()) *
                AngleAxisd(x(2), Vector3d::UnitZ());
    Vector3d t = x.segment<3>(3);
    for (int i = 0; i < traj2.size(); i++) {
        Point3D& pt = traj2[i];
        Vector3d p(pt.x, pt.y, pt.z);
        p = R * p + t;
        pt.x = p(0);
        pt.y = p(1);
        pt.z = p(2);
    }
}

int main() {
    // 构造两条轨迹
    std::vector<Point3D> traj1 = {{0, 0, 0}, {1, 1, 1}, {2, 2, 2}};
    std::vector<Point3D> traj2 = {{0, 0, 0}, {2, 2, 2}, {4, 4, 4}};

    // 对齐
    align(traj1, traj2);

    // 打印结果
    std::cout << "Trajectory 1:" << std::endl;
    for (auto pt : traj1) {
        std::cout << pt.x << ", " << pt.y << ", " << pt.z << std::endl;
    }
    std::cout << "Trajectory 2:" << std::endl;
    for (auto pt : traj2) {
        std::cout << pt.x << ", " << pt.y << ", " << pt.z << std::endl;
    }

    return 0;
}