CODEWITHSUNDEEP
c#c++3deigenquaternions
anti
anti

Reputation: 3125

Eigen Point-to-Plane algorithm, return quaternions?

I am using Eigen to calculate the best fit of a set of points to a plane. What I need to do with this data, is then rotate the set of points so they lie flat, negating the rotation value.

My code is:

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

Then, in C#, I convert the angle values to a quaternion, to rotate my object:

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

This gives me:

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

Which, when i apply it, looks nothing like it should. (I expect a roughly 45 degree rotation in one axis, I get a 180 degree flip)

I have tried switching the axes to check for coordinate space mismatch(which is likely), but I cannot get this to work. Am I doing something wrong?

I have checked the 3d points that i pass into the algorithm, and they are correct, so my issue is either in the point-to-plane code, or the quaternion conversion. Any help would be much appreciated. Thank you.

Upvotes: 0

Views: 908

Answers (1)

chtz
chtz

Reputation: 18827

If you want to calculate the quaternion which rotates one plane to another, simply compute the quaternion that rotates the normal to the other:

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

Also, don't store your angles in degrees, ever (it may sometimes make sense to output them in degrees ...).

And more importantly: Stop using Euler Angles!

Upvotes: 2

Related Questions