MVBB too large

[abranden]

Hey,
I am currently trying to calculate the MVBB for each link of a robotic arm. For most of the links, the output is OK, but some inputs produce degenerate boxes like this:

The potential MVBB is the big grey box, the actual link is displayed inside the red rectangle. I read the points of the link directly from the STL file, which I checked, does not contain any unwanted points that could cause this problem.
Generally, I just use the same approach as in the example

    // Read the points into m_points
    // [...]

    m_oobb = ApproxMVBB::approximateMVBB(m_points,0.001,500,5,0,5);

    // Make sure all points are inside oobb
    ApproxMVBB::Matrix33 A_KI = m_oobb.m_q_KI.matrix().transpose();
    auto size = m_points.cols();
    for( unsigned int i=0;  i<size; ++i ) {
        m_oobb.unite(A_KI*m_points.col(i));
    }

    // [...]
    m_box_center = m_oobb.center();
    m_box_size = m_oobb.extent();

    // [...]
    m_box_center = (m_oobb.m_q_KI * m_box_center).eval();
    m_box_size = (m_oobb.m_q_KI * m_box_size).eval();

I've also tried with different parametrizations of the algorithm by, e.g. taking 0.8 of the points as samples, or increasing number of grid-search runs.

Additionally, it does not seem to fit the link correctly, as shown in the below image, although I explicitly united all points with the oobb.

The thing that bugs me the most is, that it works for many links, but for some it just produces boxes that are way too large. I already tried to wrap my head around this problem, but failed to come up with a cause that makes only a part of the links fail.
Do you have any idea what might be causing this problem? I have the feeling I am missing something obvious.

Best regards,
André

[gabyx]

What I see is, you are (probably) doing some wrong calculations:

 m_box_center = m_oobb.center(); // In the Coordinatesystem `K` of the Box
 m_box_size = m_oobb.extent(); // In the Coordinatesystem `K` of the Box
...
m_box_size = (m_oobb.m_q_KI * m_box_size).eval(); // Wrong!

You need to make sure you are properly plotting the box.
What you need to do is

Matrix33 A_IK = m_oobb.m_q_KI.matrix(); 
// Transforms points from the Box Coordinatesystem `K` to the World System `I`.

xAxis = A_IK.col(0); // X-Axis of the Box in the World System `I`.
yAxis = A_IK.col(1); // Y-Axis of the Box in the World System `I`.
zAxis = A_IK.col(2); // Z-Axis of the Box in the World System `I`.

You plot now the Box in the WorldFrame I around the center

centerInWorldFrame = (m_oobb.m_q_KI * m_oobb.center()).eval();
with the x,y,zAxis above and the extent m_oobb.extent().

[abranden]

Thanks for the quick reply.
You can find one of the "problematic" parts here (CSV format): points.txt
After looking into the transformations, I do agree there's something wrong with it, still I can not figure out the exact problem. I am working with an interface which expects an origin (m_center), the box dimensions (m_box_size) and a rotation in roll, pitch and yaw (m_rpy). The rotation is just the rotation between K and I.
Let's define the transformations first

// From I (origin) to K (box)
ApproxMVBB::Matrix33 A_IK = m_oobb.m_q_KI.matrix().transpose();
// From K to I
ApproxMVBB::Matrix33 A_KI = m_oobb.m_q_KI.matrix();

I calculate the center via

m_box_center = (A_KI * m_oobb.center()).eval();

So far so good, I don't see where this should be wrong. The size of the box, if I understood your comment correctly, simply calculates via

m_box_size = (m_oobb.extent());

assuming A_KI does only rotate (and does not scale). Now I only have to define the rotation

Eigen::Vector3d ea = A_IK.eulerAngles(2,1,0); 
m_rpy = ea.reverse();

where ea gives me yaw, roll, pitch - the RPY reversed.

This way I should have transformed the center into I, extracted the dimensions and rotated these dimensions into I via the calculated angles.

I hope my explanation is not too intertwined - I just try to explain everything that might be the cause. Thank you very much for your time!

[gabyx]

I write you back tomorrow, there are still misunderstandings of the transformation. I will explain later... Von meinem iPhone gesendet

…

Am 12.09.2018 um 17:17 schrieb abranden ***@***.***>: Thanks for the quick reply. You can find one of the "problematic" parts here (CSV format): points.txt After looking into the transformations, I do agree there's something wrong with it, still I can not figure out the exact problem. I am working with an interface which expects an origin (m_center), the box dimensions (m_box_size) and a rotation in roll, pitch and yaw (m_rpy). The rotation is just the rotation between K and I. Let's define the transformations first // From I (origin) to K (box) ApproxMVBB::Matrix33 A_IK = m_oobb.m_q_KI.matrix().transpose(); // From K to I ApproxMVBB::Matrix33 A_KI = m_oobb.m_q_KI.matrix(); I calculate the center via m_box_center = (A_KI * m_oobb.center()).eval(); So far so good, I don't see where this should be wrong. The size of the box, if I understood your comment correctly, simply calculates via m_box_size = (m_oobb.extent()); assuming A_KI does only rotate (and does not scale). Now I only have to define the rotation Eigen::Vector3d ea = A_IK.eulerAngles(2,1,0); m_rpy = ea.reverse(); where ea gives me yaw, roll, pitch - the RPY reversed. This way I should have transformed the center into I, extracted the dimensions and rotated these dimensions into I via the calculated angles. I hope my explanation is not too intertwined - I just try to explain everything that might be the cause. Thank you very much for your time! — You are receiving this because you commented. Reply to this email directly, view it on GitHub, or mute the thread.

[gabyx]

Ok, the CoordinateTransformation A_IK (a pure Rotation!, (meaning no scaling)) corresponds to the quaternion m_oobb.m_q_KI (Rotation of the Coordinate-System I to the CoordinateSystem K see HERE).
The Matrix A_IK is used in this way:

Vector3 I_pos, K_pos;
I_pos = A_IK * K_pos; // I write it like this, because the Indices `K` cancel ;-)

Meaning it transforms a CoordinateTuple K_pos in CoordinateSystme K to a CoordinateTuple in CoordinateSystem I_pos.
So your first code should be:

// CoordinateTransformation From K to I
ApproxMVBB::Matrix33 A_IK = m_oobb.m_q_KI.matrix();
// CoordinateTransformation From I (origin) to K (box)
ApproxMVBB::Matrix33 A_KI = A_IK .transpose(); // Yes because its a rotation only (it was a unit quaternion which encodes only rotations)!

Then the center in CoordinateSystem I (WorldFrame) is:

m_box_center = (A_KI * m_oobb.center()).eval(); // which is like you computed

Then you compute roll/pitch/yaw from the quaternion rotation m_oobb.m_q_KI (Rotation which rotates CoordinateSystem I to K.
Assuming z -> y′ -> x″-Konvention (yaw, then pitch, and then roll, intrinsic!! rotating always around the current frame)

Eigen::Vector3d ypr = m_oobb.m_q_KI.eulerAngles(0,1,2);  
// Meaning m_oobb.m_q_KI == 
//        AngleAxisf(ypr[0], Vector3f::UnitX())
//     * AngleAxisf(ypr[1], Vector3f::UnitY())
//     * AngleAxisf(ypr[2], Vector3f::UnitZ());

Dont reverse the angles and think of it as the transposed rotation!! (its not true), (brain mess)
So now its up to your library you use, which euler angle definition it uses, hopefull it uses the one I described. Hard to say, because there are so many definitions... I would try the above first

[gabyx]

If you have still problems reopen this issue...