具体推导过程请参考我的博客 【附优化方法的ICP源码】ICP与NDT匹配算法精度对比，以及手动实现的ICP和基于优化方法的ICP精度对比

这里只是给出了完整的代码，如果你需要用在你自己的项目中，你需要做一些很小的改动，类似于加上CMakeListst.txt文件，以及可能需要面临头文件路径的修改，总之需要修改的地方很容易。