工程中替换非模板类Sophus为模板类Sophus

@工程中替换非模板类Sophus为模板类Sophus

解决什么问题

github上克隆了一个工程代码，在make的时候报错，错误的提示是没有sophus/so3.h这个文件，按照流程sophus是安装在usr/local/sophus下面了。但是报找不到，且安装的是比较老的非模板类sophus。
其中尝试了重新安装，以及在camkelist里面指定路径。还是报找不到sophus的头文件。后面尝试把工程里面的非模板类换成模板类。

解决方法

一、安装模板类sophus
1、核对eigen版本>3.3：链接: link.
2、参考这篇帖子安装模板类的sophus：链接: link
3、在CMakeList.txt里面完成库的链接
二、工程里面代码更改
1、将.h换成.hpp

#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

2、模板类之所以叫模板类是因为其在申明一个新的实例时，需要指定数据类型，因此，我们只需要在每一个李代数定义的地方

Sophus::SO3 SO3_v
更改为
Sophus::SO3<double> SO3_v

3、

Sophus::SE3::
更改为
Sophus::SE3d::

重新构建包。