代码之家 › 专栏 › 技术社区 › anti

特征点到平面算法,返回四元数?

quaternions eigen 3d c++ c#

anti · 技术社区 · 7 年前

我的代码是:

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度翻转)

任何帮助都将不胜感激。非常感谢。

1 回复 | 直到 7 年前

chtz 7 年前

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

#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';
}

此外,永远不要以度为单位存储角度(有时以度为单位输出可能有意义…)。

更重要的是: