hanzheteng / color_icp

Implementation of ICCV 2017: Colored Point Cloud Registration Revisited

color_icp

Introduction

This repository serves as a learning material for ICP and Colored ICP algorithms. The code is well organized and clean. We aim to focus on only the main pipeline of the algorithm, and avoid complicated interfaces and nested templates as in large libraries such as PCL and Open3D.

• The ICP algorithm follows a typical pipeline, and our implementation herein can achieve exactly the same behavior/performance as in PCL.
• The Colored ICP algorithm is an implementation of the paper: Colored Point Cloud Registration Revisited, Jaesik Park, Qian-Yi Zhou and Vladlen Koltun, ICCV 2017
• The original/official implementation of the Colored ICP algorithm is available at Open3D Github Repository. Part of the implementation in this repository has been merged into the Open3D library (See Open3D PR#4988).
• We provide some notes to discuss the math used in the Colored ICP algorithm, in particular Residuals and Jacobian matrices.

Code Structure

• config folder
  • params.yaml The YAML file to control the running flow of the point cloud registration process. We adopt a header-only library mini-yaml in this project. It is convenient for tuning parameters without the need of re-compilation of the C++ program.
• data folder
  • Contain a few sample point clouds from Redwood Synthetic and Redwood Scan datasets.
• include folder
  • color_icp/helper.h Provide some helper functions that were developed in some other projects of mine. Only the loadPointCloud function is used in this project. Feel free to make use of the rest of helper functions as you see fit.
  • color_icp/remove_nan.h Include some customized functions to remove NaN points in the point cloud; they are modified from PCL.
  • color_icp/yaml.h The header file adopted from the mini-yaml library.
• scripts folder
  • colored_icp.py A python script that runs ICP and Colored ICP algorithms using the API provided by Open3D. It can be used to compare the performance of our code with that of Open3D.
• src folder
  • color_icp.cpp The core implementation of the registration pipeline. It takes in the params.yaml file and runs modular-designed functions accordingly.
  • optimization.cpp A simple practice code to solve a curve fitting problem using Gauss-Newton method.
  • yaml.cpp The cpp file adopted from the mini-yaml library.
• Notes_on_Colored_Point_Cloud_Registration.pdf Some math notes about residuals and Jacobian matrices used in the Colored ICP algorithm.

Build and Run

The code was developed under Ubuntu 18. When needed, PCL 1.8 is used (the default version under Ubuntu 18). We follow a typical compilation procedure using CMake.

mkdir build
cd build
cmake ..
make

The running flow is controlled via the params.yaml file under the config folder. Modify the YAML file as you like, and run

./color_icp

Debugging

• During my experiments, float precision was not good enough and can cause numerical instability at convergence. This can be observed in the JTJ and JTr matrices. Switching to double precision solved this issue.

color_icp's Issues

How to tune parameters & Estimating normal vectors and curvature before executing functions

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?

the score of color_icp

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

when I run the code , error is coming

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?

Comparison with open3d official code

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.

MIT license?

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.

Multi-scale Registration in Open3D even lower the performance

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.

Jacobian Computation

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

The accuracy of the Redwood Indoor Dataset's groundtruth

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.