建立3D best fit alignment的必要条件

huangyhg

为什么近似在一条直线上的3个3维点无法建立出best fit alignment？至少需要几个点呢？

huangyhg

一、刚体变换的自由度有 6 个

在 3D 空间中，一个刚体变换（Rigid Transform）包含：

    3 个平移自由度（X, Y, Z）

    3 个旋转自由度（绕 X、Y、Z 轴）

因此，我们至少需要：

    足够数量的点（通常是 ≥ 3 个）

    且这些点在空间中不共线、不共面，才能唯一确定一个刚体变换。

huangyhg

二、3 个点共线的限制

假设你有 3 个共线的点：

    所有点都位于一条空间直线上（例如：X 轴方向）。

    它们对 绕该直线方向的旋转 不敏感 —— 任意绕该轴旋转，点集间相对位置不变。

这意味着：

    缺失一个自由度的约束，刚体解不唯一！

即使你使用拟合算法（如 Kabsch 或 SVD），它也会返回一个不确定解或数值不稳定。

huangyhg

二、3 个点共线的限制

假设你有 3 个共线的点：

    所有点都位于一条空间直线上（例如：X 轴方向）。

    它们对 绕该直线方向的旋转 不敏感 —— 任意绕该轴旋转，点集间相对位置不变。

这意味着：

    缺失一个自由度的约束，刚体解不唯一！

即使你使用拟合算法（如 Kabsch 或 SVD），它也会返回一个不确定解或数值不稳定。

huangyhg

enum class TransformationType {
  RotateAndTranslate,
  TranslateOnly,
  RotateOnly,
  PartialRotation  // ✅ 新增：可配置旋转轴的约束
};

huangyhg

新增约束：PartialRotation

你可以通过传参指定允许的旋转轴，如 allowedAxes = {false, false, true} 表示 仅绕 Z 轴旋转。
🔧 修改接口：支持传入轴限制

修改 transformationConstraint() 函数签名：

huangyhg

新增约束：PartialRotation

你可以通过传参指定允许的旋转轴，如 allowedAxes = {false, false, true} 表示 仅绕 Z 轴旋转。
🔧 修改接口：支持传入轴限制

修改 transformationConstraint() 函数签名：

huangyhg

static DenseOptimization::VectorFunction transformationConstraint(
    TransformationType transformationType,
    const std::vector<bool>& allowedRotationAxes = {true, true, true})  // 默认为全部允许旋转

huangyhg

添加 PartialRotation 分支实现：

case TransformationType::PartialRotation:
{
    // 检查输入合法
    if (allowedRotationAxes.size() != 3)
        throw std::invalid_argument("allowedRotationAxes must have 3 elements");

    std::vector<int> constrainedAxes;
    for (int i = 0; i < 3; ++i)
    {
        if (!allowedRotationAxes)
            constrainedAxes.push_back(i); // 约束这个轴为 0
    }

    const int numConstraints = static_cast<int>(constrainedAxes.size()) + 1;  // +1 for quaternion norm
    result.m = numConstraints;

    result.gradientFunction = [constrainedAxes](DenseOptimization::ConstVectorRef x,
                                                DenseOptimization::VectorRef outValues,
                                                DenseOptimization::MatrixRef outJacobian) -> void
    {
        // 平移部分放前 3 个，旋转部分从第 3 开始，四元数 qx,qy,qz,qw
        // 假设 quaternion 从 x[3] 到 x[6]
        outValues.setZero();
        outValues[0] = x.tail<4>().squaredNorm() - 1.0;

        outJacobian.setZero();
        outJacobian.bottomRows<4>().col(0) = 2.0 * x.tail<4>();  // &#8706;(q^2 - 1)/&#8706;q

        // 添加轴约束
        for (size_t i = 0; i < constrainedAxes.size(); ++i)
        {
            const int idx = constrainedAxes; // 0:x, 1:y, 2:z
            outValues[1 + i] = x[3 + idx];  // qx/qy/qz == 0
            outJacobian(3 + idx, 1 + i) = 1.0;
        }
    };

    result.hessianFunction = [constrainedAxes](DenseOptimization::ConstVectorRef /*x*/,
                                               DenseOptimization::ConstVectorRef multiplier,
                                               DenseOptimization::MatrixRef outHessian) -> void
    {
        outHessian.setZero();
        outHessian.diagonal().tail<4>().setConstant(2.0 * multiplier[0]);
        // 其他部分为常数约束，Hessian为0
    };
    break;
}

&#9989; 示例调用：

auto constraints = OptimizableTransformDefinitions<3>::transformationConstraint(
    TransformationType::PartialRotation,
    {false, false, true}  // 只允许 Z 轴旋转
);

&#129504; 说明：

    默认使用四元数旋转（四维）。

    该约束函数添加了：

        单位四元数约束（归一化）

        指定轴为 0 的额外约束（如 qx = 0，代表无 X 轴旋转）