hanzheteng / color_icp Goto Github PK

Thank you for giving us wonderful tools. I have been applying this system to our research project.
I have two questions.

1. Could you give me some advice about how to tune parameters?
   My pcd's dataset is here. And then, mismatch1.pcd and mismatch2.pcd are input to the function through param.yaml file.
   Even if Optimization has been converged, some error (like doubled object) is included as seen in the following image.
   
   Therefore, I would like to ask you how to tune some parameters to correct this error.

2. Do I estimate a normal vector and curvature of the input point cloud before inputting the point cloud?
   Some warnings occur when I execute color_icp. My dataset does not include a normal vector and curvature.
   
   However, these parameters seem to be calculated in estimateNormals() function located at 94 line, color_icp.cpp. Therefore, do I need to estimate a normal vector and curvature of the input point cloud before inputting the point cloud?

Hello,

Firstly, I would like to express my gratitude for your work. I am currently using the Redwood Indoor Dataset to train and test the D3Feat pointcloud registration method. However, as I was making groundtruth matching point pairs for training, I noticed that the groundtruth camera pose may not be accurate.

Upon visualizing the reconstruction point cloud and the point cloud generated from RGBD fragments with the provided pose, I noticed a distinct interval between the two point clouds. I am unsure whether this error is caused by the depth sensor or reconstruction method. Additionally, the error is not consistent across all fragments.

I would greatly appreciate any advice or insight you may have on this problem. Thank you for your time and assistance.

hello, I try to run the code : ./color_icp and I have set the configuration environment. but Error reporting：Section error (core has been dumped)
Is any my operations wrong?

Actually, the issue #2 also happens in the script. ICP performs better than colored_ICP in registration between color_bin_00_easy.pcd and color_bin_01.pcd. Your comment makes me convinced that there are some scenarios not proper for colored method, thanks.

Note: you can use the following json setting to view the Open3D's view (Ctrl + V on the view window). Manifest difference can be found on the boundary of the armchair.

{
	"class_name" : "ViewTrajectory",
	"interval" : 29,
	"is_loop" : false,
	"trajectory" : 
	[
		{
			"boundingbox_max" : [ 3.43359375, 2.7008078098297119, 2.1282944679260254 ],
			"boundingbox_min" : [ -0.30465653538703918, 0.89872455596923828, 0.47970101237297058 ],
			"field_of_view" : 60.0,
			"front" : [ 0.72311997525859728, -0.12254874450305511, -0.67976415513230082 ],
			"lookat" : [ 1.5644686073064804, 1.7997661828994751, 1.303997740149498 ],
			"up" : [ -0.27748035007253208, -0.95277190584179838, -0.12341130727049508 ],
			"zoom" : 0.099999999999999617
		}
	],
	"version_major" : 1,
	"version_minor" : 0
}

What makes me confused is the problem disappered when I use your color_icp.cpp file to register. It seems to show the multi-scale scheme for Colored ICP even lower its performance.

I'm assuming this code is provided under the same MIT license as Open3D?
Just checking since there is no license file in this repository.

Hello, I would like to ask how the score of this algorithm should be given, similar to the icp. getFitnessScore() in icp

Hello, thank you very much for your guidance.

I used the color_icp algorithm of your recurrence, but the effect of using your algorithm on my dataset is worse than that in official open3d. I don't know why this problem occurs.

This is the download link of my own dataset. If you can, you can download and try it. I can get good results in the official open3d code, but I can't get the same results in your code. If you can get the same results as the open3d official after trying, please tell me how to implement it. Thank you very much.

my own dataset: https://github.com/wangzixiang99/file/tree/main

I'm sorry to have taken up your time. I look forward to your reply.

@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.