Transformation concatenation along shortest path; either I'm doing something wrong or Envire Core

[HWiese1980]

Hi everyone,

I'm trying to use Envire Core to calculate transformations between coordinate systems. It seems, though, that there is either some usage error on my side, or Envire Core does something wrong when calculating indirect transforms along a shortest path.

What I'm trying to achieve is the following:

I have three (example) frames, neck, shoulder, map, where map is the root/world frame and neck and shoulder both are defined as children of map. I want to use Envire Core to give me a transformation matrix of shoulder relative to neck.

It seems though that either I've gotten something wrong about the usage/notation of the API or Envire Core has a bug when it comes to calculating indirect transforms.

My 3D affine transformation matrix notation looks as follows:

[ [R_00, R_01, R_02, T_x],
  [R_10, R_11, R_12, T_y],
  [R_20, R_21, R_22, T_z],
  [0   , 0   , 0   , 1  ] ]

i.e. the translation vector in the rightmost column. According to Wikipedia and other sources this is the common notation for a 3D affine transformation matrix. What does Envire Core expect here?

Now, I'm actually doing the following (pseudocode):

neck_mat = <some transformation matrix of neck relative to map>;
shoulder_mat = <some transformation matrix of shoulder relative to map>;

graph.addTransform("neck", "map", neck_mat);
graph.addTransform("shoulder", "map", shoulder_mat);

shoulder_in_neck_mat = graph.getTransform("shoulder", "neck");

Unfortunately shoulder_in_neck_mat comes out wrong. To me it looks like the concatenation of the matrices along the path between shoulder and neck happens in the wrong order.

Where's the error? Is it me or is it Envire Core?

Cheers,
Hendrik

[HWiese1980]

To add to this: what's maybe interesting is that I use base::Quaterniond and base::Vector3d generated from the matrix. It might be that the Quaternion is wrong due to a wrong notation.

Okay, nope, it's not the quaternion. It must be something else.

[chhtz]

It's been a while since I used this, but I think you need to switch the frames when adding transforms, i.e., origin refers to the root frame, target to the child frame (I'd need to check the source to be sure though).

[HWiese1980]

Hmm, okay, that's a bit ugly in my humble opinion because I understand that the transform defines a transformation operation of a point from the origin to the target. That is, from the child to the root/parent. Or am I wrong?

I'll check with the switched frames and report back.

Update

Nope, does not change much. I get different results but still wrong.

Here's some example code that I use for experiments around this:

#include <iostream>
#include <envire_core/graph/EnvireGraph.hpp>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Dense>

using namespace envire::core;

int main() {
    EnvireGraph _g;

    Eigen::IOFormat CleanFmt(2, 0, ", ", "\n", "[", "]");

    FrameId f_shoulder("shoulder");
    FrameId f_map("map");
    FrameId f_neck("neck");

    Eigen::Matrix4d neck_mat;
    neck_mat	<<	0.87, -0.50, 0.00, 1.00,
                    0.50,  0.87, 0.00, 0.00,
                    0.00,  0.00, 1.00, 1.50,
                    0.00,  0.00, 0.00, 1.00;

    std::cout << "Neck in map Transformation Matrix" << std::endl << neck_mat.format(CleanFmt) << std::endl << std::endl;

    base::Quaterniond neck_quat(Eigen::Matrix3d(neck_mat.topLeftCorner(3, 3)));
    base::Vector3d neck_pos(Eigen::Vector3d(neck_mat.rightCols(1).topRows(3)));
    Transform neck_in_map(neck_pos, neck_quat);
    std::cout << "Neck in map transform from Matrix" << neck_in_map.toString() << std::endl << std::endl;

    Eigen::Matrix4d shoulder_mat;
    shoulder_mat	<<	1.00, 0.00, 0.00,  1.08,
                        0.00, 1.00, 0.00, -0.13,
                        0.00, 0.00, 1.00,  1.50,
                        0.00, 0.00, 0.00,  1.00;

    std::cout << "Shoulder in map Transformation Matrix" << std::endl << shoulder_mat.format(CleanFmt) << std::endl << std::endl;

    base::Quaterniond shoulder_quat(Eigen::Matrix3d(shoulder_mat.topLeftCorner(3, 3)));
    base::Vector3d shoulder_pos(Eigen::Vector3d(shoulder_mat.rightCols(1).topRows(3)));
    Transform shoulder_in_map(shoulder_pos, shoulder_quat);
    std::cout << "Shoulder in map transform from Matrix" << shoulder_in_map.toString() << std::endl << std::endl;

    // Add Shoulder in map
    _g.addTransform(f_shoulder, f_map, shoulder_in_map);

    // Add Neck in map
    _g.addTransform(f_neck, f_map, neck_in_map);

    Transform shoulder_in_neck_a2b = _g.getTransform(f_shoulder, f_neck);
    std::cout << "Shoulder in neck Transform (from `getTransform`; wrong): " << shoulder_in_neck_a2b.toString() << std::endl << std::endl;

    // manual

    Transform map_in_neck = _g.getTransform(f_map, f_neck);
    Transform shoulder_in_neck_manual_correct = map_in_neck * shoulder_in_map;
    Transform shoulder_in_neck_manual_wrong = shoulder_in_map * map_in_neck;

    std::cout << "Shoulder Transform in neck (manual; right result!): " << shoulder_in_neck_manual_correct.toString() << std::endl << std::endl;
    std::cout << "Shoulder Transform in neck (manual; wrong result!): " << shoulder_in_neck_manual_wrong.toString() << std::endl << std::endl;

    // manual through matrices

    auto map_in_neck_mat = neck_mat.inverse();

    auto shoulder_in_neck_mat = map_in_neck_mat * shoulder_mat;


    std::cout << "Shoulder Transform in neck (matrix multiplication; correct and expected result!)" << std::endl << shoulder_in_neck_mat.format(CleanFmt) << std::endl << std::endl;

    return 0;
}

[HWiese1980]

I was able to work around the issue by fetching the direct transforms shoulder in map and neck in map, inverting neck in map to map in neck (matrix inversion) and concatenating them manually in reverse order: transform = matmul(map_in_neck, shoulder_in_map). Now it works as intended.

That does not fix the actual issue that I have with Envire Core. It's just a workaround for cases like mine where the graph tree is only a stump.

So the issue is still valid and I'll leave it open! Either I don't use Envire Core as intended or it has a (pretty severe) bug. Further investigation is required.

[HWiese1980]

@arneboe In case that you might miss this

[planthaber]

Envire was build Data driven (Environment Representation), so i currently expect, that getTransform("map","neck") returns the transformation to convert Data from "map" to "neck", so it is map_to_neck and not map_in_neck as you expected