Unknown x2.5 scaling issue in trajectory,

[adamanov]

Describe the bug

Unknown scaling issue in trajectory, which could not be resolved. I have already tried 2-3 different camera parameter configuration
one from default rs-enumerate-devices -c, and another one. Therefore, I do not think such a scaling issue could related to camera parameters.

To Reproduce

config file:

Camera:
  name: "D455 stereo"
  setup: "stereo"
  model: "perspective"


  fx: 391.343017578125 # from rs-enumarate -c
  fy: 391.343017578125
  cx: 322.371795654297
  cy: 180.013961791992

  # there is no distortion after stereo rectification

  k1: 0.0
  k2: 0.0
  p1: 0.0
  p2: 0.0
  k3: 0.0

  # focal_x_baseline is -P2[0][3] which is calculated with cv::stereoRectify()

  fps: 30.0
  cols: 640  # 
  rows: 480  #
  focal_x_baseline: 94.923 # in mm
  depth_threshold: 40

  color_order: "Gray"

# original intrinsic parameters (K, D) and stereo-recitification parameters (R)
# matrices (K, R) are written in row-major order

StereoRectifier:
  K_left: 
    [
      391.343017578125, 0.0,           322.371795654297,     # fx  0    px
      0.0,           391.343017578125, 180.013961791992,     #     fy   py
      0.0,           0.0,           1.0                #          1 
    ]
  D_left: [0.0, 0.0, 0.0, 0.0, 0.0]
  R_left:
    [
      1.0, 0.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0,
    ]
  K_right: 
    [
      391.343017578125,    0.0,           322.371795654297, 
      0.0,              391.343017578125, 180.013961791992, 
      0.0,              0.0,           1.0
    ]
  D_right: [0.0, 0.0, 0.0, 0.0, 0.0]
  R_right:
    [
      1.0, 0.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0,
    ]

Preprocessing:
  min_size: 800

Feature:
  name: "default ORB feature extraction setting"
  scale_factor: 1.2
  num_levels: 8
  ini_fast_threshold: 20
  min_fast_threshold: 7

Mapping:
  backend: "g2o"
  baseline_dist_thr: 0.11007784219
  redundant_obs_ratio_thr: 0.9
  erase_temporal_keyframes: false
  num_temporal_keyframes: 15

Tracking:
  backend: "g2o"
  enable_temporal_keyframe_only_tracking: false
  margin_last_frame_projection: 10.0
  enable_auto_relocalization: true
  max_num_local_keyfrms: 500

KeyframeInserter:
  wait_for_local_bundle_adjustment: false

Relocalizer:
  search_neighbor: true

LoopDetector:
  backend: "g2o"

Initializer:
  min_num_triangulated_pts: 100

System:
  map_format: "msgpack"
  num_grid_cols: 47
  num_grid_rows: 30

Expected behavior

Unknown scaling issue in trajectory, which could not be resolved. I am already tried 2-3 different camera parameter configuration
one from default rs-enumerate-devices -c, and another one

Screenshots or videos

Results in moving toward x-direction with interval 2meters, however, you can see from below trajectory output, it moves 5 meters each time. Same effect is recorder whenever I tested y-direction movement. It clearly indicate that there is x2.5 factor scaling issue.

Example of test setup

Environment

• Hardware: Msi laptop and D455 realsense
• CPU: Intel Core i7-11700
• OS: Ubuntu 22.04
• Commit id: [e.g. stella_vslam=577cb04d9245aa68b0905212293a802292d01787, stella_vslam_ros=fdbd287d80a3a5cc4ac1a143dc937adb4697b120]
• Install procedure: Native
• dataset: real world experience

[adamanov]

Update

I tested now with a different resolution i.e. 1080x720, and I do see a scale of x1.5 in trajectory. Therefore I guess i missing some settings?

again, each time I moved 2 meters, but recorded trajectory is x1.5 factor

[ymd-stella]

I can advise you to do it with stereo setup, but I recommend using RGBD setup for D455.

[adamanov]

I can advise you to do it with stereo setup, but I recommend using RGBD setup for D455.

hello, just had a chance to test it with the RGBD channel, it does not work at all
this conflict which I used is similar to the previous 1280x720 version, just changed line setup: "RGBD"

Camera:
  name: "D455 stereo"
  setup: "RGBD"
  model: "perspective"

  fx: 652.238342285156 # from rs-enumarate -c
  fy: 652.238342285156
  cx: 644.743591308594
  cy: 360.027923583984

  # there is no distortion after stereo rectification

  k1: 0.0
  k2: 0.0
  p1: 0.0
  p2: 0.0
  k3: 0.0

  # focal_x_baseline is -P2[0][3] which is calculated with cv::stereoRectify()

  fps: 30.0
  cols: 1280  # 
  rows: 720  #
  focal_x_baseline: 94.923 # in mm (for D455 cam)
  depth_threshold: 40

  color_order: "Gray"

# original intrinsic parameters (K, D) and stereo-recitification parameters (R)
# matrices (K, R) are written in row-major order

StereoRectifier:
  model: "perspective"
  K_left: 
    [
      652.238342285156, 0.0,           644.743591308594,     # fx  0    px
      0.0,           652.238342285156, 360.027923583984,     #     fy   py
      0.0,           0.0,           1.0                #          1 
    ]
  D_left: [0.0, 0.0, 0.0, 0.0, 0.0]
  R_left:
    [
      1.0, 0.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0,
    ]
  K_right: 
    [
      652.238342285156,    0.0,           644.743591308594, 
      0.0,              652.238342285156, 360.027923583984, 
      0.0,              0.0,           1.0
    ]
  D_right: [0.0, 0.0, 0.0, 0.0, 0.0]
  R_right:
    [
      1.0, 0.0, 0.0,
      0.0, 1.0, 0.0,
      0.0, 0.0, 1.0,
    ]

here is the video
Screencast from 08-30-2024 10:19:38 AM.webm,

Since my application is in-out/door, I plan to use stereo version of vslam rather than RGBD which is more suitable for indoor applications. However, from the video, you can see that after it reinitialized and the movement began, the algorithm cannot track at all...

do you have any idea, what might be scaling issue in the stereo setup? I assume it somehow related to resolution of the camera?...

[adamanov]

i guess i found out the problem, focal_x_baseline is equal to multiplication of fx and baseline distance between cameras.

I forgot to multiply it with fx and entered 94.923 in mm (for D455 cam), which is baseline distance between two cameras in realsense only