1、原理介绍

2、代码介绍

     基于C++编写的程序代码如下，其依赖eigen矩阵运算库，在创建工程时包含库目录中使用了相对路径，因此其下载下来直接可以运行，不用单独在设置环境，非常方便。