CSDN链接，详情见csdn链接，我的这个csdn链接写的更加详细一些。如有问题，请提issue或者在csdn评论区交流。代码中保留了lio-sam中的四个cpp，但是把这四个cpp的编译删除了，主要是为了和自己写的gtsam做对比，方便学习。
代码运行逻辑：
mkdir src
cd src
git clone git@github.com:zhao-zhibo/test_gtsam.git
cd ..
catkin_make
最近这一周一直在学习lio-sam，顺便把gtsam再学习下。学习流程主要是先学习了非线性优化部分，分别写了gaussNewton.cpp、testGtsam.cpp、curveFitting.cpp三个cpp。后面代码上传到我的GitHub上，目前还未上传，注意我的代码中依赖了matplotlibcpp，没有这个库的同学直接在头文件中删除它即可。
| cpp名称 | 具体内容 |
|---|---|
| gaussNewton.cpp | 手写高斯牛顿法（视觉slam十四讲书中的曲线拟合133页曲线拟合指数函数），参考了b站讲解，链接主要是帮助理解高斯牛顿增量方程中的累加，因为原始式子（1）是没有累加的，但是在实际计算时式子（2）进行了累加。 |
| testGtsam.cpp | 复现gtsam官网中的BetweenFactor，特别是自定义因子UnaryFactor，参考gtsam文档 |
| curveFitting.cpp | 用gtsam实现视觉slam十四讲133页的曲线拟合指数函数，构造自定义因子，参考gtsam曲线拟合 |
其中在写前两个cpp时没有遇到特别的问题，写curveFitting.cpp时遇到了一些问题，第一个是推导雅克比矩阵，在自定义因子的时候，需要推导出误差方程的雅克比矩阵，这部分遇到了比较多的的问题。第二个是代码运行过程中遇到的两个问题，这两个问题分别记录到下面1.3中.
重写高斯牛顿法
在对二维刚体变换的推导过程中遇到了问题，因为自定义因子时，需要用到误差方程的雅克比矩阵，因此推导雅克比矩阵的时候遇到了问题，参考gtsam曲线拟合，这里面做了推导。
这个cpp是curveFitting.cpp，其中结合了matplotlibcpp.h，matplotlibcpp.h是安装的c++画图的依赖库，方便画图使用，安装参考链接，这里面和1.1节中一样，都是对曲线进行拟合，因此它的雅克比矩阵也是现成的，具体可以参考下面代码中的evaluateError函数。可以见代码curveFitting.cpp。
最终拟合后的曲线如下图所示：
在写自定义因子的时候踩了两个坑，分别在1.31.和1.3.2中进行介绍。
第一个错误是在上述代码中的 // return gtsam::Vector1(val - yi); // 返回值为残差，这样写会返回错误，
lio_sam_curveFitting: /usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h:241: Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index) [with _Scalar = double; int _Rows = 1; int _Cols = 1; int _Options = 0; int _MaxRows = 1; int _MaxCols = 1; Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index = long int]: ' Assertion SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed. [lio_sam_curveFitting-2] process has died [pid 1903900, exit code -6, cmd /home/zhao/Codes/competition_code/lio-sam-mergePoints/devel/lib/lio_sam/lio_sam_curveFitting __name:=lio_sam_curveFitting __log:=/home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2.log]. log file: /home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2*.log
最后的解决方式在见stackoverflow，这个是我在stackoverflow提的问题，别人回答说主要是Eigen的版本太老了或者gtsam依赖的太老了。主要是更换了返回的方式，如下
gtsam::Vector1 ret;
ret[0] = val - yi;
return ret;这个问题的主要干扰项是 ros::init(argc, argv, "lio_sam");，它会影响gtsam中的优化函数optimize()的使用，目前的解决方案是直接删除这一行代码，要不然会报错，报错如下：
