Hydra ICP under unkown scaling / shift

[mhubii]

Implement a version of Hydra ICP that supports relative depth estimates from models such as Depth Anything v2 or MIDAS.

Targeting 0.4.6 without downstream integration as redundant with available RGB-D cameras and ros2_roboreg support for them. Purely research-based with potential future integration.

[mhubii]

as per @tvercaut's proposal:

• Init scale of bias of depth map
• Init pose
• while ICP nor converged
  • find NN assignments
  • compute robust standard deviation of residuals
  • while loop 2 not converged
  • compute robust weights
  • while loop 3 not converged
    • solve for pose increment (linear least squares 1)
    • solve for depth scale and bias (linear least squares 2)
  • apply pose increment

Further notes re depth mapping:

Let a pixel with coordinates [u_i, v_i] have a relative depth d_i.
Let [x_i, y_i, z_i] be the coordinates of the corresponding 3D point.
Let s and b be the unknown scale and bias of the depth.
Let f be the known focal length of the camera.
The absolute depth and 3D coordinates are related by: (sd_i + b)^2 = x_i^2 + y_i^2 + z_i^2
By projective geometry we have: u_i = (f/z_i) x_i and v_i = (f/z_i) y_i
Equivalently: x_i = (z_i/f)u_i and y_i = (z_i/f)v_i
We thus have x_i^2 + y_i^2 + z_i^2 = ( (u_i/f)^2 + (v_i/f)^2 + 1 )z_i^2
Which gives: z_i = (sd_i + b) / sqrt( (u_i/f)^2 + (v_i/f)^2 + 1 )
We now have expressed [x_i, y_i, z_i] as a function of the observed values (u_i, v_i, d_i) and the unknown scale and bias terms (s, b)
This is a linear equation in terms of s and b but it gets multiplied by the transformation matrix in the P2P

[tvercaut]

I started to put a few rough notes together in section 1.1 here:
https://www.overleaf.com/read/gvqktdppcsgy#595559
It's very much WIP

[mhyatt000]

@mhubii I am looking into this also. DAv2 claims to use 'metric depth' finetuned model, so i wonder if that would help

[mhubii]

That would be great if you could share any findings!

Back then, we decided to not investigate further since the differentiable rendering algorithms in this library do not require depth.

[mhyatt000]

in my case depth anything had worked pretty well but could be improved

[mhyatt000]

i was not the most diligent about producing great segmentation masks

[mhyatt000]

i can pr a script to add this to the pipeline if it would be useful

[tvercaut]

To improve from there, it could help to refine the scaling and offset in the estimated depth. Essentially you would be using "metric" monocular estimation to initialise the algorithm and then only consider it as "relative" depth estimation.

From your current results, you should be able to implement (a variant of) the suggestion above pretty easily.

Input: Approximate metric depth points from the segmented robot image (d_i, u_i, v_i); camera focal length f
Initialisation: Set s←1 and o←0
Step 1: Transform the (d_i, u_i, v_i) depth+image coordinates in Euclidean coordinates and run Hydra ICP to get a pose estimation. (This is what you already did above)
Step 2a: From the pose estimation, compute the mesh points (from the transformed CAD model) in the 3D camera coordinate system, keeping track of index correspondences from the ICP (and the robustness weights ω_i). Let's refer to these points as \tilde{p}_i with i correponding to the approximate metric depth points indices
Step 2b: Assemble a U matrix but stacking the U_i (from depth+image coordinates) matrices below. Also stack the transformed mesh points \tilde{p}_i into a large vector p

Step 2b: Use linear least squares to solve for ξ=[s; o] in p≈Uξ (s should be close to 1 and o close to 0 if the initial metric depth was quite good). For improved robustness, a weighted linear least squares can be exploited by reusing the robustness weights ω_i from Hydra ICP in Step 1
Step 2c: Set d_i ← s * d_i + o
Run steps 1 and 2 several times (until convergence)