Rotate translation vector to robot coordinates

[maxspier]

No description provided.

[cgpadwick]

let's use cv::Mat to represent the rotation matrix and cv::Vec3d for the translation and the apriltag vector.

https://docs.opencv.org/3.4/dc/d84/group__core__basic.html#gab9e0ab642a3e01742916763173b72232

https://docs.opencv.org/3.4/dc/d84/group__core__basic.html#gab9e0ab642a3e01742916763173b72232

[cgpadwick]

this isn't quite right. we want to multiply the april tag vector by the R matrix and then add the translation, like this v' = R * v + T.

Let's use the open cv matrices cv::Mat and cv::Vec3d instead of multiplying them by hand. OpenCV has optimized matrix multiply implementations that are much faster than a hand multiply (and it's easier to read).

[cgpadwick]

I was thinking something like this:

cv::Mat matrix = (cv::Mat_<double>(3, 3) << 
                       1, 2, 3,
                       4, 5, 6,
                       7, 8, 9);

    // 2. Create a 3D vector
    cv::Vec3d vec(1.0, 2.0, 3.0);

    // 3. Convert the cv::Vec3d into a 3x1 cv::Mat (column vector)
    cv::Mat vecMat = cv::Mat(vec); // This becomes a 3x1 matrix

    // 4. Multiply the 3x3 matrix by the 3x1 vector
    cv::Mat resultMat = matrix * vecMat; // Result will be a 3x1 matrix

[maxspier]

Makes sense. Just changed the double array to the different data structure.

Left a todo for crystal to add the translation stuff (they were doing that).

[cgpadwick]

looks good, thanks Max! We'll merge it as is and then perhaps later read translations/rotations from a different file but this will do for now