`keypoint` doesn't match `local_orientation_per_joint' plus 'local_position_per_joint'

Software
1

Hi, I’m experiencing minor difference between ‘keypoint’ and ‘loca_…_per_joint’ and wonder what I have done wrong, or if it’s expected.

In this video, the purple cones are placed directly in the scene without nesting. I position them in the world 3D coordinate base on the keypoint returned from the SDK; Meanwhile the yellow dots are 2D overlay, calculate based on camera projection

Meanwhile in this version, I nest the purple cones following the body_38 structure, then applied local_orientation_per_joint and local_position_per_joint to each cones. The nested cones are put in a container which is rotated by global_root_orientation, and transformed by position.

As you’ll notice, the purple cones and yellow dots don’t align correctly in the second video.

In case that matters, I’m using python SDK to export the data, and render it in a three.js (OpenGL) application. The coordinate system is “RIGHT_HANDED_Y_UP”.

2

Hi,

the local rotations/positions are relative to their parent, not the root. If you want to get the world position from a local one, you need to apply the transform of each of its parents, not only the transform of the root.

Stereolabs Support

3

Hi @BenjaminV ,

It’s indeed what I’ve been trying to do. Please see my code below (please ignore the syntax and just focus on the logic):

// create 38 bones
const bone0 = new THREE.Object3D(...)
const bone1 = new THREE.Object3D(...)
const bone2 = new THREE.Object3D(...)
...

// put all bones in an array
const bones = [bone0, bone1, bone2, bone3, bone4, ...]

// setup bone structure
bone2.add(bone3)
bone1.add(bone2)
...
bone0.add([bone1, bone18, bone19])
avatar.add(bone0)

// assign position and rotation on each blone
bones.forEach((bone, idx) => {
  const [x, y, z, w] = bodyData.local_orientation_per_joint[idx]
  const [px, py, pz] = bodyData.local_position_per_joint[idx]
  let quat = new THREE.Quaternion(x, y, z, w)
  bone.rotation.setFromQuaternion(quat)
  bone.position.set(px, py, pz)
})

// assign global position and rotation on avatar
const [grx, gry, grz, grw] = bodyData.global_root_orientation
const [gpx, gpy, gpz] = bodyData?.position
avatar.position?.set(gpx, gpy, gpz)
avatar.rotation?.setFromQuaternion(new THREE.Quaternion(grx, gry, grz, grw))

Meanwhile this is a snaphot of bodyData:

{
    "id": 0,
    "unique_object_id": "6a525f1f-2154-41d2-937c-0cd1f431f03c",
    "tracking_state": "OK",
    "action_state": "Unknown",
    "position": [
        -0.17757096886634827,
        -0.1935255378484726,
        -1.8625593185424805
    ],
    "keypoint": [
        [
            -0.17392665147781372,
            -0.13637541234493256,
            -2.234830856323242
        ],
        [
            -0.17300370335578918,
            -0.04928351193666458,
            -2.2448554039001465
        ],
        [
            -0.17051489651203156,
            0.11204149574041367,
            -2.266720771789551
        ],
        [
            -0.17215241491794586,
            0.27327093482017517,
            -2.2893576622009277
        ],
        [
            -0.1757638156414032,
            0.4841286242008209,
            -2.318679094314575
        ],
        [
            -0.13617512583732605,
            0.6494736671447754,
            -2.168818950653076
        ],
        [
            -0.09912426769733429,
            0.7196698784828186,
            -2.1946513652801514
        ],
        [
            -0.16447752714157104,
            0.7263635396957397,
            -2.184772253036499
        ],
        [
            -0.07516886293888092,
            0.6902915239334106,
            -2.3141040802001953
        ],
        [
            -0.22765980660915375,
            0.7059100866317749,
            -2.29105281829834
        ],
        [
            -0.09759770333766937,
            0.4149583578109741,
            -2.37099289894104
        ],
        [
            -0.2577924132347107,
            0.4134228229522705,
            -2.3623046875
        ],
        [
            0.025395214557647705,
            0.4392699599266052,
            -2.3578662872314453
        ],
        [
            -0.38122889399528503,
            0.4280070960521698,
            -2.3412961959838867
        ],
        [
            0.21595634520053864,
            0.21263591945171356,
            -2.473909616470337
        ],
        [
            -0.5609250664710999,
            0.17918626964092255,
            -2.2579894065856934
        ],
        [
            0.33111572265625,
            0.1648889034986496,
            -2.18133282661438
        ],
        [
            -0.4846339821815491,
            0.18033888936042786,
            -1.9492485523223877
        ],
        [
            -0.06720247864723206,
            -0.20788466930389404,
            -2.236344814300537
        ],
        [
            -0.28211915493011475,
            -0.20342284440994263,
            -2.2173683643341064
        ],
        [
            -0.10273293405771255,
            -0.7281732559204102,
            -2.276541233062744
        ],
        [
            -0.3863876760005951,
            -0.7155122756958008,
            -2.195669412612915
        ],
        [
            -0.16045424342155457,
            -1.1991297006607056,
            -2.3225395679473877
        ],
        [
            -0.42846956849098206,
            -1.1890668869018555,
            -2.2306361198425293
        ],
        [
            -0.06956416368484497,
            -1.2357745170593262,
            -2.158151388168335
        ],
        [
            -0.5224908590316772,
            -1.215987205505371,
            -2.069215774536133
        ],
        [
            -0.04418449103832245,
            -1.249598741531372,
            -2.2509567737579346
        ],
        [
            -0.5562507510185242,
            -1.2194151878356934,
            -2.160301685333252
        ],
        [
            -0.20697557926177979,
            -1.280045509338379,
            -2.362083911895752
        ],
        [
            -0.40115663409233093,
            -1.274897575378418,
            -2.2771384716033936
        ],
        [
            0.31278207898139954,
            0.10068166255950928,
            -2.056358575820923
        ],
        [
            -0.39171338081359863,
            0.1890583336353302,
            -1.8426324129104614
        ],
        [
            0.3611845374107361,
            0.11690641194581985,
            -2.0718700885772705
        ],
        [
            -0.44345659017562866,
            0.19933569431304932,
            -1.8346532583236694
        ],
        [
            0.40987130999565125,
            0.061769939959049225,
            -2.0215024948120117
        ],
        [
            -0.4462820291519165,
            0.1832956075668335,
            -1.747005820274353
        ],
        [
            0.39878565073013306,
            0.07534001767635345,
            -2.1221842765808105
        ],
        [
            -0.4912570118904114,
            0.14143551886081696,
            -1.8286689519882202
        ]
    ],
    "local_position_per_joint": [
        [
            0.0,
            0.0,
            0.0
        ],
        [
            0.0,
            0.1098049208521843,
            0.0
        ],
        [
            0.0,
            0.20392341911792755,
            0.0
        ],
        [
            0.0,
            0.20392341911792755,
            0.0
        ],
        [
            0.0,
            0.2666690945625305,
            0.0
        ],
        [
            0.0,
            0.2180270403623581,
            -0.09698665142059326
        ],
        [
            -0.03677874431014061,
            0.06540811061859131,
            0.04849332571029663
        ],
        [
            0.03677874431014061,
            0.06540811061859131,
            0.04849332571029663
        ],
        [
            -0.049038324505090714,
            -0.06540811061859131,
            0.10910999029874802
        ],
        [
            0.049038324505090714,
            -0.06540811061859131,
            0.10910999029874802
        ],
        [
            -0.09192918241024017,
            0.1882370114326477,
            0.06549114733934402
        ],
        [
            0.09192918241024017,
            0.1882370114326477,
            0.06549114733934402
        ],
        [
            -0.14446014165878296,
            0.0,
            0.0
        ],
        [
            0.14446014165878296,
            0.0,
            0.0
        ],
        [
            -0.3890097439289093,
            0.0,
            0.0
        ],
        [
            0.3890097439289093,
            0.0,
            0.0
        ],
        [
            -0.3890097439289093,
            0.0,
            0.0
        ],
        [
            0.3890097439289093,
            0.0,
            0.0
        ],
        [
            -0.12935879826545715,
            -0.0810602679848671,
            0.0
        ],
        [
            0.12935879826545715,
            -0.0810602679848671,
            0.0
        ],
        [
            0.0,
            -0.6079519987106323,
            0.0
        ],
        [
            0.0,
            -0.6079519987106323,
            0.0
        ],
        [
            0.01437320001423359,
            -0.5539118051528931,
            0.0
        ],
        [
            -0.01437320001423359,
            -0.5539118051528931,
            0.0
        ],
        [
            0.05749280005693436,
            -0.0810602679848671,
            -0.21045707166194916
        ],
        [
            -0.04311959818005562,
            -0.0810602679848671,
            -0.21045707166194916
        ],
        [
            -0.04311959818005562,
            -0.0810602679848671,
            -0.15032647550106049
        ],
        [
            0.05749280005693436,
            -0.0810602679848671,
            -0.15032647550106049
        ],
        [
            0.0,
            -0.0810602679848671,
            0.09019588679075241
        ],
        [
            0.0,
            -0.0810602679848671,
            0.09019588679075241
        ],
        [
            -0.12879522144794464,
            -0.04564013332128525,
            -0.06806962937116623
        ],
        [
            0.12879522144794464,
            -0.04564013332128525,
            -0.06806962937116623
        ],
        [
            -0.12879522144794464,
            0.0,
            -0.034034814685583115
        ],
        [
            0.12879522144794464,
            0.0,
            -0.034034814685583115
        ],
        [
            -0.22246447205543518,
            0.0,
            -0.011344937607645988
        ],
        [
            0.22246447205543518,
            0.0,
            -0.011344937607645988
        ],
        [
            -0.12879522144794464,
            -0.011410033330321312,
            0.045379750430583954
        ],
        [
            0.12879522144794464,
            -0.011410033330321312,
            0.045379750430583954
        ]
    ],
    "local_orientation_per_joint": [
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.009839312173426151,
            -0.011564291082322598,
            0.003159652464091778,
            0.9998797178268433
        ],
        [
            0.0031450274400413036,
            -0.006824038457125425,
            -0.012505057267844677,
            0.9998936057090759
        ],
        [
            -0.00047616034862585366,
            -0.0005061055417172611,
            -0.0034824125468730927,
            0.9999936819076538
        ],
        [
            -0.23850063979625702,
            0.041822969913482666,
            0.0598614364862442,
            0.9683929681777954
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            -0.06988206505775452,
            -0.08513209968805313,
            -0.0868607684969902,
            0.9901132583618164
        ],
        [
            -0.06167449802160263,
            0.06155506893992424,
            0.058233924210071564,
            0.9944928884506226
        ],
        [
            0.0070016407407820225,
            0.249116450548172,
            0.4830579459667206,
            0.839372992515564
        ],
        [
            0.012048646807670593,
            0.08301841467618942,
            -0.48979490995407104,
            0.8677924275398254
        ],
        [
            0.0,
            -0.7112389206886292,
            0.0,
            0.7029504179954529
        ],
        [
            0.0,
            0.6648617386817932,
            0.0,
            0.7469663619995117
        ],
        [
            -0.09633965790271759,
            0.17458310723304749,
            0.11465919017791748,
            0.9731869101524353
        ],
        [
            -0.08588491380214691,
            -0.04051445797085762,
            0.025958286598324776,
            0.9951424598693848
        ],
        [
            -0.09895340353250504,
            0.32796913385391235,
            -0.013082875870168209,
            0.9394004940986633
        ],
        [
            -0.010459241457283497,
            -0.336098313331604,
            0.1009075865149498,
            0.936347246170044
        ],
        [
            -0.025619545951485634,
            0.0,
            0.0,
            0.9996717572212219
        ],
        [
            -0.07922334969043732,
            0.0,
            0.0,
            0.9968568682670593
        ],
        [
            0.1608084738254547,
            0.010664582252502441,
            0.03505411371588707,
            0.986305296421051
        ],
        [
            0.12870816886425018,
            -0.043464936316013336,
            -0.00998961552977562,
            0.990679144859314
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ],
        [
            0.0,
            0.0,
            0.0,
            1.0
        ]
    ],
    "global_root_orientation": [
        0.007821538485586643,
        0.9973514080047607,
        -0.05697409063577652,
        -0.04453147202730179
    ]
}

And thanks for the quick reply!

4

is bone.position the local or global position of the bone ?

5

calling bone.position.set(…) change it’s position in it’s parent’s local coordinate.

6

So under this setup:

scene.add(bone0)
bone0.add(bone1)
bone1.add(bone2)

bone0.position.set(1, 0, 0)
bone1.position.set(1, 0, 0)
bone2.position.set(1, 0, 0)

bone2 will end up at (3, 0, 0) in global coordinate

7

Are you sure you have to set both the position and the rotation?

8

Hi @BenjaminV yes I’m sure.

If you take a look at the video, you can see the yellow points (absolute keypoint coordinate) are quite close to the purple joints (calculated from orientation and position).

I’m just wondering if it is due to rounding error, or if I misunderstand the data structure.

9

the cones seems to be placed correctly relative to each other, but they are not placed correctly in the world coordinate system.

I think the issue is in the avatar.position?.set(gpx, gpy, gpz)

What is the origin of your avatar?

10

The “avatar” is just an empty “GROUP”, placed directly in the 3D scene without any parent or scaling. So its origin would be (0,0,0).

But actually the cones don’t exactly match the yellow points. There are some differences in their scale (the cone seems wider apart then the yellow dots).

This lead me to believe this might be rounding issue…(or not?)