2
$\begingroup$

I am using KITTI odometry dataset whereby the camera's axis is pointing X (east), Y (down) and Z (north). It also contain the frame to frame poses of the camera (first pose is fixed as the map frame).

I am using the formula below to obtain the matrix that would get me from a point on the ground surface to uv coordinate.

Matrix<float,3,4>  3d_to_2s = calibration.get_proj2() * calibration.get_rect0() * T_cam0unrect_road;

whereby get_proj2() is the projection matrix, get_rect0() is the rectifying matrix and T_cam0unrect_road is extrinsic matrix to get from ground to where the camera is mounted (camera is initially slightly pitched).

AngleAxisf m = AngleAxisf(0.03, Vector3f::UnitX());
T_cam0unrect_road = Matrix4f::Identity();
T_cam0unrect_road(1, 3) = m_height;
T_cam0unrect_road.topLeftCorner<3,3>() = m.matrix();

Question

Because the vehicle is moving at high speed, the camera is pitching up and down between frames. I wish to solve this by compensating the pitch of the camera with the following code instead,

Vector3f triplets = pose.topLeftCorner<3,3>().eulerAngles(2, 1, 0);
AngleAxisf m = AngleAxisf(0.03 + triplets(2) , Vector3f::UnitX());

However, when I printed out the rotations (in degrees) below, I don't get sensible values for pitch (X axis). I am unsure if this could be due to the heading (Y axis) of the camera moving close to 90 degrees.

So, how should I resolve this problem of compensating for the pitch when I get strange values like this? I understand that using quarternion or rotation matrices would be better, but I thought I just need to compensate for the pitching of the camera.

processing frame 0
rotation around Z: 5.18165e-10
rotation around Y: -1.33316e-09
rotation around X: -1.37073e-08
processing frame 1
rotation around Z: 0.100961
rotation around Y: 2.49588
rotation around X: 0.147765
processing frame 2
rotation around Z: 0.345722
rotation around Y: 5.0829
rotation around X: 0.430607
processing frame 3
rotation around Z: 0.430648
rotation around Y: 7.71205
rotation around X: 0.69544
processing frame 4
rotation around Z: 0.359564
rotation around Y: 10.3777
rotation around X: 0.86194
processing frame 5
rotation around Z: 0.55802
rotation around Y: 13.0701
rotation around X: 0.903135
processing frame 6
rotation around Z: 0.692939
rotation around Y: 15.7423
rotation around X: 0.997595
processing frame 7
rotation around Z: 0.958596
rotation around Y: 18.3829
rotation around X: 1.27789
processing frame 8
rotation around Z: 1.27656
rotation around Y: 20.9566
rotation around X: 1.54352
processing frame 9
rotation around Z: 1.36157
rotation around Y: 23.587
rotation around X: 1.64277
processing frame 10
rotation around Z: 1.49622
rotation around Y: 26.2514
rotation around X: 1.68765
processing frame 11
rotation around Z: 1.60329
rotation around Y: 28.9723
rotation around X: 1.71041
processing frame 12
rotation around Z: 1.69588
rotation around Y: 31.7076
rotation around X: 1.75604
processing frame 13
rotation around Z: 1.873
rotation around Y: 34.4132
rotation around X: 1.89264
processing frame 14
rotation around Z: 2.2461
rotation around Y: 37.0944
rotation around X: 2.18429
processing frame 15
rotation around Z: 2.37565
rotation around Y: 39.7527
rotation around X: 2.36015
processing frame 16
rotation around Z: 2.88682
rotation around Y: 42.4081
rotation around X: 2.72221
processing frame 17
rotation around Z: 3.10883
rotation around Y: 45.073
rotation around X: 2.92285
processing frame 18
rotation around Z: 3.28406
rotation around Y: 47.7952
rotation around X: 3.12997
processing frame 19
rotation around Z: 3.92615
rotation around Y: 50.4643
rotation around X: 3.76177
processing frame 20
rotation around Z: 4.23934
rotation around Y: 53.1277
rotation around X: 4.12701
processing frame 21
rotation around Z: 4.81483
rotation around Y: 55.7626
rotation around X: 4.73797
processing frame 22
rotation around Z: 5.79948
rotation around Y: 58.3712
rotation around X: 5.85173
processing frame 23
rotation around Z: 6.4649
rotation around Y: 60.9555
rotation around X: 6.52549
processing frame 24
rotation around Z: 7.65324
rotation around Y: 63.4012
rotation around X: 7.5979
processing frame 25
rotation around Z: 8.85895
rotation around Y: 65.7174
rotation around X: 8.72029
processing frame 26
rotation around Z: 10.0229
rotation around Y: 67.8928
rotation around X: 9.97569
processing frame 27
rotation around Z: 11.247
rotation around Y: 69.9217
rotation around X: 11.1209
processing frame 28
rotation around Z: 12.809
rotation around Y: 71.8836
rotation around X: 12.4875
processing frame 29
rotation around Z: 14.4739
rotation around Y: 73.806
rotation around X: 14.1026
processing frame 30
rotation around Z: 15.8785
rotation around Y: 75.7117
rotation around X: 15.5913
processing frame 31
rotation around Z: 18.1652
rotation around Y: 77.4908
rotation around X: 17.9393
processing frame 32
rotation around Z: 21.6176
rotation around Y: 79.1577
rotation around X: 21.3585
processing frame 33
rotation around Z: 26.2236
rotation around Y: 80.7618
rotation around X: 25.8872
processing frame 34
rotation around Z: 31.5809
rotation around Y: 82.3621
rotation around X: 31.1235
processing frame 35
rotation around Z: 40.0482
rotation around Y: 83.7397
rotation around X: 39.5374
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.