Eigen Point-to-Plane算法，return四元数？

Question

我正在使用 Eigen 来计算一组点与平面的最佳拟合。我需要用这些数据做的是旋转点集，使它们平放，否定旋转值。

我的代码是：

cv::Point2f plane_from_points(const std::vector<Vector3> & c)
{
    // copy coordinates to  matrix in Eigen format
    size_t num_atoms = c.size();
    Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > coord(3, num_atoms);
    for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i];

    // calculate centroid
    Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());

    // subtract centroid
    coord.row(0).array() -= centroid(0); coord.row(1).array() -= centroid(1); coord.row(2).array() -= centroid(2);

    // we only need the left-singular matrix here
    //  http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points

    auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    Vector3 plane_normal = svd.matrixU().rightCols<1>();

    float x = plane_normal[0];
    float y = plane_normal[1];
    float z = plane_normal[2];


    float angle = atan2(x, z) * 180 / PI;
    float angle2 = atan2(y, z) * 180 / PI;    

    cv::Point ret(angle, angle2);    
    return ret;
}

然后，在 C# 中，我将角度值转换为四元数，以旋转我的对象：

  public static Quaternion QuatFromEuler(double yaw, double pitch, double roll)
        {
            yaw = Deg2Rad(yaw);
            pitch = Deg2Rad(pitch);
            roll = Deg2Rad(roll);
            double rollOver2 = roll * 0.5f;
            double sinRollOver2 = (double)Math.Sin((double)rollOver2);
            double cosRollOver2 = (double)Math.Cos((double)rollOver2);
            double pitchOver2 = pitch * 0.5f;
            double sinPitchOver2 = (double)Math.Sin((double)pitchOver2);
            double cosPitchOver2 = (double)Math.Cos((double)pitchOver2);
            double yawOver2 = yaw * 0.5f;
            double sinYawOver2 = (double)Math.Sin((double)yawOver2);
            double cosYawOver2 = (double)Math.Cos((double)yawOver2);
            Quaternion result = new Quaternion();
            result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
            result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
            result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
            result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;

            return result;
        }

这给了我：

angles: -177 -126
quat: -0.453834928533952,-0.890701198505913,-0.0233238317256566,0.0118840858439476

当我应用它时，它看起来并不像它应该的那样。（我预计在一个轴上大约旋转 45 度，我得到 180 度翻转）

我已经尝试切换轴来检查坐标 space 不匹配（这很可能），但我无法让它工作。我做错了什么吗？

我已经检查了我传递给算法的 3d 点，它们是正确的，所以我的问题要么在点到平面代码中，要么在四元数转换中。任何帮助将非常感激。谢谢。

Answer 1

如果要计算将一个平面旋转到另一个平面的四元数，只需计算将法线旋转到另一个平面的四元数：

#include <Eigen/Geometry>

int main() {
    using namespace Eigen;
    // replace this by your actual plane normal:
    Vector3d plane_normal = Vector3d::Random().normalized();
    // Quaternion which rotates plane_normal to UnitZ, or the plane to the XY-plane:
    Quaterniond rotQ = Quaterniond::FromTwoVectors(plane_normal, Vector3d::UnitZ());

    std::cout << "Random plane_normal: " << plane_normal.transpose() << '\n';
    std::cout << "rotated plane_normal: " << (rotQ * plane_normal).transpose() << '\n';
}

此外，永远不要以度为单位存储角度（有时以度为单位输出它们可能有意义...）。

更重要的是：停止使用欧拉角！

Eigen Point-to-Plane算法，return四元数？

Eigen Point-to-Plane algorithm, return quaternions?

c#

c++

3d

quaternions

eigen