Jacobian Computation

[gitouni]

@hanzheteng , I have a question about Jacobian computation here.

  Eigen::Matrix6d JTJ_G = JacobianGeo * JacobianGeo.transpose();
  Eigen::Vector6d JTr_G = JacobianGeo * ResidualGeo;
  Eigen::Matrix6d JTJ_C = JacobianColor * JacobianColor.transpose();
  Eigen::Vector6d JTr_C = JacobianColor * ResidualColor;

  Eigen::Matrix6d JTJ = sqrt(lambda) * JTJ_G + sqrt(1-lambda) * JTJ_C;
  Eigen::Vector6d JTr = sqrt(lambda) * JTr_G + sqrt(1-lambda) * JTr_C;

I assume the weights should be added only on JTr rather than both on JTJ and JTr. My reasons are below.
In the supplementary materials of Colored ICP, the original residual function is:

$$ E(T)=(1-\sigma)\sum_x{(r_C(T))^2}+\sigma \sum_x{(r_G(T))^2} $$

The aggregated residual and Jacobian are:

$$J=[J_G, J_C], r=[ \sqrt{\sigma}* r_G, \sqrt{1-\sigma}* r_C]$$

For C++ implementation, it should be:

  Eigen::MatrixXd Jacobain(6, 2*size);
  Eigen::VectorXd Residual(2*size);
  Jacobain << JacobianGeo, JacobianColor; // concatenate columns
  Residual << sqrt(lambda)*ResidualGeo, sqrt(1-lambda)*ResidualColor;  // concatenate columns
  Eigen::MatrixXd JJT = Jacobain * Jacobain.transpose();
  Eigen::MatrixXd Jr = Jacobain * Residual;
  Eigen::Vector6d X = JJT.ldlt().solve(-Jr);

This is equivalent to adding weights on JTr but not on JTJ. Due to the combined rows and matrix operation,
$$JJ^T=J_GJ_G^T+J_CJ_C^T$$.

I believe your implementation is right, because I also implement it with ceres-solver's Autodifferentiable function and its result is more similar to yours. However, I still have no idea about what is wrong.

[hanzheteng]

The aggregated residual and Jacobian are:

I don't know how you derived the equation after this sentence, but Jacobians are supposed to have the same weights as residuals, since they are related. Please see equation (28-30) in the paper for their relation. Also, please see equation (22) and (25) in the original paper, where you can see that the weights lambda and 1-lambda are multiplied to both J and r, and therefore both JTJ and JTr.

On the other hand, you can think about the intuition behind it. It does not make sense to add weights to only residuals but not Jacobians. Leaving Jacobians with no weights is equivalent to assigning a weight of one, which makes it inconsistent with the weights of residuals.

Besides, the equation you pointed out in the supplementary is the objective function for RGB-D alignment. The authors wanted to say that they borrowed the idea from RGB-D alignment and applied it to point cloud registration. The corresponding objective function is listed in equation (17) in the paper.

[gitouni]

My apologies for this miskate. You're right. Weights should be added to the Jacbobian because the residual is the function and the weighted function will affect the Jacobian too. However, I still think there is a issue here: the weight should be $\sigma$ and $1-\sigma$ with respect to $JJ^T$ and $Jr$ rather than their squared root.

As you mentioned before, the core of this reisdual function is listed in equation (17). So the aggregated residual function is:

$$r =\begin{bmatrix}\sqrt{1-\sigma} \cdot r_C\quad; \sqrt{\sigma} \cdot r_G\end{bmatrix}\qquad, E(T)=\Vert r(T)\Vert_2^2$$

Suppose the respective Jacobian of $r_C(T)$ and $r_G(T)$ are $J_C$ and $J_G$, then the aggregated Jacboain of $r(T)$ is

$$J^T=\begin{bmatrix}\sqrt{1-\sigma} \cdot J_C\quad; \sqrt{\sigma} \cdot J_G\end{bmatrix}$$

the aggregated Gaussian-Newton Iteration equation is:

$JJ^TX=Jr$

where

$$JJ^T=\begin{bmatrix}\sqrt{1-\sigma} \cdot J_C\quad; \sqrt{\sigma} \cdot J_G\end{bmatrix}^T\cdot \begin{bmatrix}\sqrt{1-\sigma} \cdot J_C\quad; \sqrt{\sigma} \cdot J_G\end{bmatrix} = (1-\sigma)\cdot J_CJ_C^T + \sigma \cdot J_GJ_G^T$$ $$Jr=\begin{bmatrix}\sqrt{1-\sigma} \cdot J_C\quad; \sqrt{\sigma} \cdot J_G\end{bmatrix}^T \cdot \begin{bmatrix}\sqrt{1-\sigma} \cdot r_C\quad; \sqrt{\sigma} \cdot r_G\end{bmatrix} = (1-\sigma)\cdot J_Cr_C + \sigma \cdot J_Gr_G$$

Your codes are:

  Eigen::Matrix6d JTJ_G = JacobianGeo * JacobianGeo.transpose();
  Eigen::Vector6d JTr_G = JacobianGeo * ResidualGeo;
  Eigen::Matrix6d JTJ_C = JacobianColor * JacobianColor.transpose();
  Eigen::Vector6d JTr_C = JacobianColor * ResidualColor;

  Eigen::Matrix6d JTJ = sqrt(lambda) * JTJ_G + sqrt(1-lambda) * JTJ_C;
  Eigen::Vector6d JTr = sqrt(lambda) * JTr_G + sqrt(1-lambda) * JTr_C;

While I think we do not need to calculate the squared root of $\lambda$ and $1-\lambda$:

  Eigen::Matrix6d JTJ_G = JacobianGeo * JacobianGeo.transpose();
  Eigen::Vector6d JTr_G = JacobianGeo * ResidualGeo;
  Eigen::Matrix6d JTJ_C = JacobianColor * JacobianColor.transpose();
  Eigen::Vector6d JTr_C = JacobianColor * ResidualColor;

  Eigen::Matrix6d JTJ = lambda * JTJ_G + (1-lambda) * JTJ_C;
  Eigen::Vector6d JTr = lambda* JTr_G + (1-lambda) * JTr_C;

Another alternative is an aggregated version:

 Jacobain << sqrt(lambda)*JacobianGeo, sqrt(1-lambda)*JacobianColor; // concatenate cols
  Residual << sqrt(lambda)*ResidualGeo, sqrt(1-lambda)*ResidualColor;
  Eigen::MatrixXd JJT = Jacobain * Jacobain.transpose();
  Eigen::MatrixXd Jr = Jacobain * Residual;