introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1.01k stars 558 forks source link

Negative stereo baseline using OAK-D PoE #1142

Open YoungCapta1n opened 7 months ago

YoungCapta1n commented 7 months ago

I am trying to follow this tutorial using OAK-D PoE camera, However, the following error [ERROR] [1712385487.272759382]: The stereo baseline (-0.074629) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg Can I solve this issue by changing the launch file? Thanks!

matlabbe commented 7 months ago

Can you show (rostopic echo ...) the camera_info for the left camera and the one for the right camera?

Shivam7Sharma commented 5 months ago

Hi,

I am also getting this error. The following are the ros2 topic echo result:

Right camera topic:

---
header:
  stamp:
    sec: 1718923245
    nanosec: 746549891
  frame_id: oak_right_camera_optical_frame
height: 800
width: 1280
distortion_model: plumb_bob
d:
- -1.1144567728042603
- 0.4457603096961975
- -0.00030109440558589995
- 0.000643925741314888
- 1.2683645486831665
k:
- 910.39697265625
- 0.0
- 646.8530883789062
- 0.0
- 910.701904296875
- 386.07025146484375
- 0.0
- 0.0
- 1.0
r:
- 0.9984344244003296
- 0.05026911944150925
- 0.024530746042728424
- -0.05104481801390648
- 0.998180627822876
- 0.03209204599261284
- -0.02287287451326847
- -0.03329396992921829
- 0.9991838335990906
p:
- 916.425482
- 52.429217
- 636.126594
- -8205.09755
- -85.9618346
- 879.762776
- 443.910167
- 270.032187
- 0.0167888962
- -0.0630299225
- 0.997870386
- -0.334740132
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false
---
header:
  stamp:
    sec: 1718923245
    nanosec: 346532658
  frame_id: oak_right_camera_optical_frame
height: 800
width: 1280
distortion_model: rational_polynomial
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 910.39697265625
- 0.0
- 646.8530883789062
- 0.0
- 910.701904296875
- 386.07025146484375
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 910.39697265625
- 0.0
- 646.8530883789062
- 79.88569861994125
- 0.0
- 910.701904296875
- 386.07025146484375
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false
---

Left camera info topic :

---
header:
  stamp:
    sec: 1718923330
    nanosec: 280492777
  frame_id: oak_left_camera_optical_frame
height: 800
width: 1280
distortion_model: plumb_bob
d:
- -1.8108996152877808
- 0.5892674922943115
- 0.00020837810006923974
- 0.0008147237240336835
- -0.4694308340549469
k:
- 906.6533813476562
- 0.0
- 625.2329711914062
- 0.0
- 906.753662109375
- 399.9358825683594
- 0.0
- 0.0
- 1.0
r:
- 0.9980300664901733
- -0.049864426255226135
- 0.03807265684008598
- 0.051083534955978394
- 0.9981896281242371
- -0.03174852952361107
- -0.036420609802007675
- 0.03363087400794029
- 0.9987704753875732
p:
- 906.65338135
- 0.0
- 625.23297119
- 0.0
- 0.0
- 906.75366211
- 399.93588257
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false
---
header:
  stamp:
    sec: 1718923330
    nanosec: 313811443
  frame_id: oak_left_camera_optical_frame
height: 800
width: 1280
distortion_model: rational_polynomial
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 906.6533813476562
- 0.0
- 625.2329711914062
- 0.0
- 906.753662109375
- 399.9358825683594
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 910.39697265625
- 0.0
- 646.8530883789062
- 0.0
- 0.0
- 910.701904296875
- 386.07025146484375
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false

The error: [stereo_odometry-1] [ERROR] [1718922309.890265790] [rtabmap.stereo_odometry]: The stereo baseline (-0.087748) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.

matlabbe commented 4 months ago

Are the two models (plumb_bob and rational_polynomial) published on same camera_info topic? or they are 4 different camera_info?

Either case, if I use for example the rational_polymonial one from the right camera, the projection matrix is:

p:
- 910.39697265625
- 0.0
- 646.8530883789062
- 79.88569861994125
- 0.0
- 910.701904296875
- 386.07025146484375
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0

Here Tx or P[0,3] is 79.88569861994125, while Fx is 910.39697265625, so baseline is -Tx/Fx = -0.087748, which is negative. We follow convention stated in CameraInfo msg description:

# Projection/camera matrix
#     [fx'  0  cx' Tx]
# P = [ 0  fy' cy' Ty]
#     [ 0   0   1   0]
# By convention, this matrix specifies the intrinsic (camera) matrix
#  of the processed (rectified) image. That is, the left 3x3 portion
#  is the normal camera intrinsic matrix for the rectified image.
# It projects 3D points in the camera coordinate frame to 2D pixel
#  coordinates using the focal lengths (fx', fy') and principal point
#  (cx', cy') - these may differ from the values in K.
# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
#  also have R = the identity and P[1:3,1:3] = K.
# For a stereo pair, the fourth column [Tx Ty 0]' is related to the
#  position of the optical center of the second camera in the first
#  camera's frame. We assume Tz = 0 so both cameras are in the same
#  stereo image plane. The first camera always has Tx = Ty = 0. For
#  the right (second) camera of a horizontal stereo pair, Ty = 0 and
#  Tx = -fx' * B, where B is the baseline between the cameras.
# Given a 3D point [X Y Z]', the projection (x, y) of the point onto
#  the rectified image is given by:
#  [u v w]' = P * [X Y Z 1]'
#         x = u / w
#         y = v / w
#  This holds for both images of a stereo pair.