C＃

我正在使用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度）

我试过切换轴来检查坐标空间不匹配（可能），但我无法得到这个工作。 难道我做错了什么？

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

---

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

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

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

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

链接地址: http://www.djcxy.com/p/81869.html

上一篇: c#

下一篇: Quaternion is flipping sign for very similar rotations?