Pointclouds in World Coordinates

[MoBaT]

I'm having trouble outputting proper point clouds into the WORLD scene.

From the pclMsg_ that gets output to the topic, I'm grabbing the X,Y,Z from fields 4-6 in the RovioNode.hpp and putting them into a pointCloudImu Vector3f. Once I have these, I am under the impression that I need to convert them from IMU coordinates to WORLD.

So I grab the "World-Frame to IMU-Frame" translation and quat from the "imuOutput_" variable inside RovioNode.hpp by doing:

Vector3f pointCloudImu(); // From the fields 4-6
Vector3f worldToImuTrans(imuOutput_.WrWB()(0), imuOutput_.WrWB()(1), imuOutput_.WrWB()(2));
Quaterneionf worldToImuQuat(-imuOutput_.qBW().w(), imuOutput_.qBW().x(), imuOutput_.qBW().y(), imuOutput_.qBW().z());

Then I do:

pointCloudWorld = (worldToImuQuat.inverse() * (pointCloudImu - worldToImuTrans));

Am I doing this correctly?

[bloesch]

Not entirely sure about the details, but the what write cannot be correct since you subtract worldToImuTrans (in world frame) from pointCloudImu (in IMU frame). You can not combine coordinate vectors which are expressed in different coordinate frame. Also, it should be a +.

I think what you should do instead is: WrWP = WrWB + qWB * BrBP, with

• BrBP: coord. vector from IMU to point (P) expressed in the IMU frame (B)
• WrWB: coord. vector from World to IMU expressed in the World frame (W)
• qWB: quaternion representing rotation from IMU to world

I do not exactly remember how to get qWB and you will have to make sure that the order of the components is correct.

[MoBaT]

Yep! I was doing it incorrectly. I got it working now!

I confirmed my answer by using 2 methods:

FIrst I got the bearing in camera and distance from the outputted PointCloud msg and calculated the CrCP. Then I compared it to the ImuPoint MrMP rotated and translated to a CrCP. If they match, then I know I'm doing something right.

CrCP1 = distance * bearingInCamera
CrCP2 = qCM * (MrMP - MrMC)

Then I rotated and translated the CrCP1 to Global Coordinates and then rotated and translated the ImuPoint to GlobalCoordinates using the respective quats and translations to see if they matched.

WrWP1 = (qWM * MrMP) + WrWM
WrWP2 = (qWC * CrCP1) + WrWC

Everything ended up being kosher! Thanks so much.