How could depth residual Jacobian Matrix be simple like that?

[gongyeted]

Hello, I am a student studying SLAM and I want to do some further research on your great work. Thank you for your open source code. I read your code carefully and don't understand the depth residual Jacobian part. In your code the depth residual Jacobian is in a very simple format, while I tried to do some mathematical derivation myself, but can't get the equation in your code. Would you please give me some advice or some reference? I noticed that in your code it says “Simplified form of the new version above by rotating all the vectors into the local frame (which does not change the values of the dot products), i.e., multiplying by frame_tr_global from the left side", but I don't really understand it.

[puzzlepaint]

In principle, the derivation of the Jacobians was mostly done with the provided Python script applications/badslam/scripts/jacobians_derivation.py. You seem to refer to the first one, taken wrt. the keyframe pose, titled "Jacobian of depth residual wrt. global_T_frame changes". Unfortunately, when I tried to re-run the script now, it gave me some NaN values as a result (perhaps due to a change in the sympy version? Not sure why it broke.) But in principle, this script should be able to compute the basic form of the Jacobian (the one named "New version" in the comments in the source code) automatically from a Python version of the residual expression.

The script operates on the following form of the residual (as written in a comment in the script), with surfel_normal in the global coordinate system:

dot(surfel_normal, global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) - surfel_pos)

The notation on the SE(3) transformations is A_T_B, where a multiplication with a point p as follows: A_T_B * p will transform it from the coordinate system B into the coordinate system A. So, A_T_B is a transformation from B to A.

With the above form of the residual, the multiplication with global_T_frame gets into the Jacobian, which causes some computations that the simplification tries to shift to surfel_normal instead. The idea is that the value of the dot product in the residual should not change if the vectors on the left and right side of the dot product are rotated in the same way. The surfel normal (on the left side) can be rotated into the local keyframe's coordinate system, and both the global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) and surfel_pos vectors (on the right side) can be individually multiplied with frame_tr_global from the left, meaning an SE(3) transformation from the global into the keyframe's local coordinate system, to rotate the result of their subtraction also into the local keyframe's coordinate system.

As a result, the global_R_frame rotation is on the surfel normal instead of on the expression exp(hat(T)) * unproject(x, y, correct(depth)) on the right side. This may be advantageous, since the rotated surfel normal got already computed elsewhere in the code, so that result could be re-used in the Jacobian computation instead of rotating the other vector.

Looking at it in another way, the expressions in the Jacobian computation code should be equivalent as well. For example, consider the first entry of the "New version" residual:

//   jacobian[0] = gtf.row0.x*surfel_global_normal.x + gtf.row1.x*surfel_global_normal.y + gtf.row2.x*surfel_global_normal.z;

Here, gtf is supposed to mean the global_T_frame transformation from the keyframe to the global frame. The rotation part of this transformation can be inverted by taking its transpose, since it is a rotation matrix. The commented code line above multiplies the surfel_global_normal vector with the first column in gtf, or equivalently, the first line in its inverse. Thus, in effect it computes surfel_local_normal.x. This is exactly the expression in the "simplified" version (plus the multiplication with depth_residual_inv_stddev, which seemingly was added to the residual after the "New version" was written down).

[gongyeted]

In principle, the derivation of the Jacobians was mostly done with the provided Python script applications/badslam/scripts/jacobians_derivation.py. You seem to refer to the first one, taken wrt. the keyframe pose, titled "Jacobian of depth residual wrt. global_T_frame changes". Unfortunately, when I tried to re-run the script now, it gave me some NaN values as a result (perhaps due to a change in the sympy version? Not sure why it broke.) But in principle, this script should be able to compute the basic form of the Jacobian (the one named "New version" in the comments in the source code) automatically from a Python version of the residual expression.

The script operates on the following form of the residual (as written in a comment in the script), with surfel_normal in the global coordinate system:

dot(surfel_normal, global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) - surfel_pos)

The notation on the SE(3) transformations is A_T_B, where a multiplication with a point p as follows: A_T_B * p will transform it from the coordinate system B into the coordinate system A. So, A_T_B is a transformation from B to A.

With the above form of the residual, the multiplication with global_T_frame gets into the Jacobian, which causes some computations that the simplification tries to shift to surfel_normal instead. The idea is that the value of the dot product in the residual should not change if the vectors on the left and right side of the dot product are rotated in the same way. The surfel normal (on the left side) can be rotated into the local keyframe's coordinate system, and both the global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) and surfel_pos vectors (on the right side) can be individually multiplied with frame_tr_global from the left, meaning an SE(3) transformation from the global into the keyframe's local coordinate system, to rotate the result of their subtraction also into the local keyframe's coordinate system.

As a result, the global_R_frame rotation is on the surfel normal instead of on the expression exp(hat(T)) * unproject(x, y, correct(depth)) on the right side. This may be advantageous, since the rotated surfel normal got already computed elsewhere in the code, so that result could be re-used in the Jacobian computation instead of rotating the other vector.

Looking at it in another way, the expressions in the Jacobian computation code should be equivalent as well. For example, consider the first entry of the "New version" residual:

//   jacobian[0] = gtf.row0.x*surfel_global_normal.x + gtf.row1.x*surfel_global_normal.y + gtf.row2.x*surfel_global_normal.z;

Here, gtf is supposed to mean the global_T_frame transformation from the keyframe to the global frame. The rotation part of this transformation can be inverted by taking its transpose, since it is a rotation matrix. The commented code line above multiplies the surfel_global_normal vector with the first column in gtf, or equivalently, the first line in its inverse. Thus, in effect it computes surfel_local_normal.x. This is exactly the expression in the "simplified" version (plus the multiplication with depth_residual_inv_stddev, which seemingly was added to the residual after the "New version" was written down).

Thank you so much for your explicit explanation and sorry for havn't been able to respond immediately. I am not very familiar with python, but I will figure out how these scripts work soon. Thank you again for your help, it really helped me a lot !

[gongyeted]

In principle, the derivation of the Jacobians was mostly done with the provided Python script applications/badslam/scripts/jacobians_derivation.py. You seem to refer to the first one, taken wrt. the keyframe pose, titled "Jacobian of depth residual wrt. global_T_frame changes". Unfortunately, when I tried to re-run the script now, it gave me some NaN values as a result (perhaps due to a change in the sympy version? Not sure why it broke.) But in principle, this script should be able to compute the basic form of the Jacobian (the one named "New version" in the comments in the source code) automatically from a Python version of the residual expression.

The script operates on the following form of the residual (as written in a comment in the script), with surfel_normal in the global coordinate system:

dot(surfel_normal, global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) - surfel_pos)

The notation on the SE(3) transformations is A_T_B, where a multiplication with a point p as follows: A_T_B * p will transform it from the coordinate system B into the coordinate system A. So, A_T_B is a transformation from B to A.

With the above form of the residual, the multiplication with global_T_frame gets into the Jacobian, which causes some computations that the simplification tries to shift to surfel_normal instead. The idea is that the value of the dot product in the residual should not change if the vectors on the left and right side of the dot product are rotated in the same way. The surfel normal (on the left side) can be rotated into the local keyframe's coordinate system, and both the global_T_frame * exp(hat(T)) * unproject(x, y, correct(depth)) and surfel_pos vectors (on the right side) can be individually multiplied with frame_tr_global from the left, meaning an SE(3) transformation from the global into the keyframe's local coordinate system, to rotate the result of their subtraction also into the local keyframe's coordinate system.

As a result, the global_R_frame rotation is on the surfel normal instead of on the expression exp(hat(T)) * unproject(x, y, correct(depth)) on the right side. This may be advantageous, since the rotated surfel normal got already computed elsewhere in the code, so that result could be re-used in the Jacobian computation instead of rotating the other vector.

Looking at it in another way, the expressions in the Jacobian computation code should be equivalent as well. For example, consider the first entry of the "New version" residual:

//   jacobian[0] = gtf.row0.x*surfel_global_normal.x + gtf.row1.x*surfel_global_normal.y + gtf.row2.x*surfel_global_normal.z;

Here, gtf is supposed to mean the global_T_frame transformation from the keyframe to the global frame. The rotation part of this transformation can be inverted by taking its transpose, since it is a rotation matrix. The commented code line above multiplies the surfel_global_normal vector with the first column in gtf, or equivalently, the first line in its inverse. Thus, in effect it computes surfel_local_normal.x. This is exactly the expression in the "simplified" version (plus the multiplication with depth_residual_inv_stddev, which seemingly was added to the residual after the "New version" was written down).

And I tried with sympy version 1.3 it worked without NaN values. You are right on the reason of script broken！

[sqn175]

Hi, using mathematical derivation, the depth residual Jacobian Matrix proves to be the same as the automatic derivation results in the python scripts. The derivation details are as follows:

[gongyeted]

Hi, using mathematical derivation, the depth residual Jacobian Matrix proves to be the same as the automatic derivation results in the python scripts. The derivation details are as follows:

Thank you for your help !