Transform Frames do not match Sensor data

seank-com commented 5 years ago

This could totally be my misunderstanding as the documentation is incredibly sparse (what is the purpose of timeThrMs?) on exactly how the [Sensor. getTfData]() function works but I could not find coordinates that match.

Expected:

Pose2D.X == Frame.BaseX

Actual

Pose2D.X != Frame.BaseX

qinyusen commented 5 years ago

Hi, this is Yusen

I'm in charge of the native implementation about this module in Segway Robitcs. I will try to help if you find some troubles when you using it.

I've saw your code with

AlgoTfData tfBase = robotSensor.getTfData(Sensor.WORLD_ODOM_ORIGIN, Sensor.BASE_ODOM_FRAME, pose2D.getTimestamp(), 100);

You can just try this , and the reason you can read the 3. :

AlgoTfData tfBase = robotSensor.getTfData(Sensor.BASE_ODOM_FRAME, Sensor.WORLD_ODOM_ORIGIN, pose2D.getTimestamp(), 100);

BTW, check the 'int err' in AlgoTfData, if err = 0 means works well, or let me know the error code.

The tf module is a sensor data hub which save and make the interpolation when you look up a frame transformation, so timeThrMs is the time threshold and unit is ms, which means if you look up for a timesamp t, all the sensors for calculating the transformation should have some effective value in [t +timeThrMs, t - timeThrMs ] , if there is not, will be a error code come out.

And here is some more info you may read before you are using the tf in Loomo: 1. https://en.wikipedia.org/wiki/Quaternion https://en.wikipedia.org/wiki/Rotation_matrix https://en.wikipedia.org/wiki/Translation_(geometry) 2. For the transformation between src frame and tgt frame, is using passive transformation。 What's the active or passive transformation：

https://en.wikipedia.org/wiki/Active_and_passive_transformation
Selig, J.M., 2006. Active versus passive transformations in robotics. IEEE Robotics & Automation Magazine, 13(1), pp.79-84.

3.All the transformation in this API should be apply the transformation(r,t) to the source frame, and you will got the target frame, i.e. src frame ==r, t==> tgt frame，so getTfData means tgt frame 's pose in src frame.
so

 AlgoTfData tfBase = robotSensor.getTfData(Sensor.BASE_ODOM_FRAME, Sensor.WORLD_ODOM_ORIGIN, pose2D.getTimestamp(), 100);

means get the tf represent, BASE_ODOM_FRAME 's pose in WORLD_ODOM_ORIGIN.

seank-com commented 5 years ago

Thanks for the links and explanations. I've been doing computer graphics since the late 90s so I am familiar with the matrix math involved and using quaternions to avoid gimbal lock.

So there are bugs in the API and documentation?

The parameters appear to be named incorrectly from your description above.

And they are described in the development docs opposite of how you described them above.

qinyusen commented 5 years ago

Sorry, I've not catch the point.

 AlgoTfData tfBase = robotSensor.getTfData(Sensor.BASE_ODOM_FRAME, Sensor.WORLD_ODOM_ORIGIN, pose2D.getTimestamp(), 100);

the params are targetFrame, sourceFrame, lookupTimestamp, and a time threshold. I don't know which you means named incrrect.

So if you want to get the base pose in world odom. The targetFrame should be BASE_ODOM_FRAME, and the sourceFrame should be WORLD_ODOM_ORIGIN.

For the document, there are mistakes, it's hasn't been update for a long time.

seank-com commented 5 years ago

OK, I must have read that wrong. Could you explain what the frames are? I guessed at a few of them for an example.

BASE_ODOM_FRAME - ?The position on the ground directly below the robot? BASE_POSE_FRAME - ? HEAD_POSE_P_FRAME - ? HEAD_POSE_P_R_FRAME - ? NECK_POSE_FRAME - ? PLATFORM_CAM_FRAME - ?The position of the camera to the right of the LCD display? HEAD_POSE_Y_FRAME - ? RS_COLOR_FRAME - ?The position of the RealSense color camera? RS_DEPTH_FRAME - ?The position of the RealSense depth camera? RS_FE_FRAME - ? WORLD_EVIO_ORIGIN - ? WORLD_ODOM_ORIGIN - ?The position of the robot the last time it set the world origin?

qinyusen commented 5 years ago

Infact this is what we only can post here, for further usage and information, I would like to give support by e-mail.

Bu the way, there was some information mismatch(i.e. a bug) about the set origin API, cause that's some java code that haven't match the document and still without any people to fix it.

WORLD_ODOM_ORIGIN is the virtual pose that the robot start. and it drift sometimes.

all the frames are under the coordinate of odom, which should be like this:

        z   x
        |  /
        | /
    y---|/

so does the RS_COLOR_FRAME and RS_DEPTH_FRAME etc.

You need to make a transformation, if you want to use the camera coordinate like this:

    +--------------------------+    +--------------------+      +-----------------------------------------+
    |                          |    |                    |      |                                         |
    | encoder based odom frame |    | vslam based frame  |      | other world frame sample                |
    |                          |    |                    |      |                                         |
    | "world_odom_frame"       |    | "world_evio_frame" |      | "the world frame related on robot base" |
    |                          |    |                    |      |                                         |
    +----------+---------------+    +---------+----------+      +----------------+------------------------+
               |                              |                                  |
               |                              |                                  |
               |                              |                                  |
               |                +-------------v----------------+                 |
               |                |                              |                 |
               +--------------> |  robot coordinate root       |                 |
                                |                              | <---------------+
                                |  "base_center_ground_frame"  |
                                |                              |
                                 +-----------^-----------------++
                                             |
                                             |
                                             |
                                             |
                                             |
                 +---------------------------+----------------+              +------------------------------------------+
                 |                                            |              |                                          |
                 |  the robot base center with pitch && roll  |              |  realsense depth camera sensor center.   |
                 |                                            |    +---------+                                          |
                 |  "base_center_wheel_axis_frame"            |    |         |  "rsdepth_center_neck_fix_frame"         |
                 |                                            |    |         |                                          |
                 +-------------+------------------------------+    |         +------------------------------------------+
                               ^                                   |
                               |                                   |         +------------------------------------------+
                               |                                   |         |                                          |
                               |                                   |         |  realsense fish eye camera sensor center |
+------------------------------+----------------------------+      |    +----+                                          |
|                                                           <------+    |    |  "rsfisheye_center_neck_fix_frame"       |
|  the neck center pose frame with yaw                      |           |    |                                          |
|  the name means this is at the neck center position,      |           |    +------------------------------------------+
|  and it's connect on the robot body with a yaw rotation.  <-----------+
|                                                           |                +------------------------------------------+
|  "neck_center_body_yaw"                                   |                |                                          |
|                                                           <----------------+  realsense color camera sensor center    |
+-------------------------+---------------------------------+                |                                          |
                          ^                                                  |  "rscolor_center_neck_fix_frame"         |
                          |                                                  |                                          |
                          |                                                  +------------------------------------------+
                          |
+-------------------------+------------------------+                         +-----------------------------------+
|                                                  |                         |                                   |
|  the head center of the axis that cross the ear  |                         | platform camera sensor center     |
|                                                  <-------------------------+                                   |
|  "head_center_neck_pitch_frame"                  |                         | "platform_center_head_fix_frame"  |
|                                                  |                         |                                   |
+--------------------------------------------------+                         +-----------------------------------+

seank-com / loomo-on-azure

Transform Frames do not match Sensor data #11

Expected:

Actual