SLAM 2、代码基础
参考这个链接下的安装路径：https://github.com/gaoxiang12/slambook2/tree/master/3rdparty
一、安装环境
安装Eigen
Eigen有效支持线性代数，矩阵和矢量运算，数值分析及其相关的算法。
安装：
sudo apt install libeigen3-dev
安装Pangolin
Pangolin是SLAM常用的库之一，主要用于SLAM系统的可视化。它是基于OpenGL的，主要优势在于使用比OpenGL简单
使用tar -zxvf解压Pangolin资源包，并先下载以下内容：
sudo apt install doxygen
sudo apt install libglew-dev
# 如果在后续安装步骤，还报错缺什么，就百度一下安装一下命令行cd到解压好的Pangolin文件夹内，按照以下步骤执行：
cd Pangolin
mkdir build && cd build
cmake -DCPP11_NO_BOOST=1 ..
make
sudo make install此时，如果你有出现以下报错：
: error: ‘PixelFormat’ does not name a type; did you mean ‘AVPixelFormat’?
PixelFormat fmtdst;
^~~~~~~~~~~
AVPixelFormat需要前往Pangolin/src底下，修改里面的CMakeLists，从第266行开始，注释以下内容：
find_package(FFMPEG QUIET)
# if(BUILD_PANGOLIN_VIDEO AND FFMPEG_FOUND)
# set(HAVE_FFMPEG 1)
# list(APPEND INTERNAL_INC ${FFMPEG_INCLUDE_DIRS} )
# list(APPEND LINK_LIBS ${FFMPEG_LIBRARIES} )
# list(APPEND HEADERS ${INCDIR}/video/drivers/ffmpeg.h)
# list(APPEND SOURCES video/drivers/ffmpeg.cpp)
# message(STATUS "ffmpeg Found and Enabled")
# endif()删除并重新建立一个build文件夹，重复上述编译安装步骤即可。
安装ceres
ceres是同g2o一样，运用在slam前端或后端，对相机位姿和路标点进行非线性寻优的算法，特别适用于解决后端Boundle Adjustment (BA)问题。
ceres可能需要的依赖项有：
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-devtar -zxvf解压程序包，执行以下操作：
cd ceres-solver
mkdir build && cd build
cmake ..
make
sudo make install安装gtsam
GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，可以解决slam和sfm的问题，当然它也可以解决简单或者更加复杂的估计问题。
因子图是一种图模型，非常适合于复杂的估计问题的建模，比如SLAM或者是SFM( Structure from Motion)。贝叶斯网络属于一种无环图模型
安装gtsam之前需要确保boost是可以正常运行的。坑比较多，有需要可以参考boost官网
tar -zxvf解压gtsam程序包，执行以下操作：
cd gtsam-4.0.2
mkdir build && cd build
cmake ..
make
sudo make install安装Sophus
Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。
Sophus依赖fmt的，所以需要先安装fmt
git clone https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install然后才安装Sophus。这Sophus源码也需要修改一下，前往Sophus/sophus/so2.cpp，对第30行开始的地方做如下修改：
SO2::SO2()
{
// unit_complex_.real() = 1.;
// unit_complex_.imag() = 0.;
unit_complex_.real(1.);
unit_complex_.imag(0.);
}然后再按规矩编译安装
cd Sophus
mkdir build
cd build
cmake -Wno-dev ..
make
sudo make install安装g2o
g2o是一个通用图优化算法库。 目前主流的SLAM研究基本都是基于图优化的
先安装一些依赖
sudo apt-get install qt5-qmake qt5-default libqglviewer-dev-qt5 libsuitesparse-dev libcxsparse3 libcholmod3 libqglviewer-dev-qt4 libqt4-dev qt4-qmake libqglviewer-headers 安装g2o
cd g2o
mkdir build
cd build
cmake ..
make
sudo make install如果中途出现找不到GL的报错，可以替换 g2o/cmake_modules里面的 FindQGLViewer.cmake文件，能够找到 libqglviewer
# Need to find both Qt{4,5} and QGLViewer if the QQL support is to be built
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtXml QtOpenGL QtGui)
IF(NOT Qt4_FOUND)
FIND_PACKAGE(Qt5 QUIET COMPONENTS Core Xml OpenGL Gui Widgets)
IF(NOT Qt4_FOUND AND NOT Qt5_FOUND)
MESSAGE("Qt{4,5} not found. Install it and set Qt{4,5}_DIR accordingly")
IF (WIN32)
MESSAGE(" In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5")
ENDIF()
ENDIF()
ENDIF()
FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h
/usr/include/QGLViewer
/opt/local/include/QGLViewer
/usr/local/include/QGLViewer
/sw/include/QGLViewer
ENV QGLVIEWERROOT
)
find_library(QGLVIEWER_LIBRARY_RELEASE
NAMES qglviewer-qt4 qglviewer QGLViewer QGLViewer2 QGLViewer-qt5
PATHS /usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
ENV QGLVIEWERROOT
ENV LD_LIBRARY_PATH
ENV LIBRARY_PATH
PATH_SUFFIXES QGLViewer QGLViewer/release
)
find_library(QGLVIEWER_LIBRARY_DEBUG
NAMES dqglviewer dQGLViewer dQGLViewer2 QGLViewerd2 QGLViewer-qt5
PATHS /usr/lib
/usr/local/lib
/opt/local/lib
/sw/lib
ENV QGLVIEWERROOT
ENV LD_LIBRARY_PATH
ENV LIBRARY_PATH
PATH_SUFFIXES QGLViewer QGLViewer/release
)
if(QGLVIEWER_LIBRARY_RELEASE)
if(QGLVIEWER_LIBRARY_DEBUG)
set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG})
else()
set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE})
endif()
endif()
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG
QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY)最后，如果你运行代码遇到诸如此类的错误：
undefined symbol: _ZN3g2o27OptimizationAlgorithmDoglegC1EPNS_15BlockSolverBaseE那是因为库没连接好，命令行运行以下语句可以解决：
export LD_LIBRARY_PATH=./lib二、代码基础
这里只先介绍Eigen和Pangolin的基础，其他部分在后续再详细展开。
参考第一篇slam三维刚体运动基础，以里面曾介绍过的数学原理为学习基础。
Eigen基本类型
示例代码中，我们会用到以下几个头文件：
#include <iostream>
// Eigen部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
// 计时用
#include <ctime>
#define MATRIX_SIZE = 50主函数中：申明一个矩阵，常用以下几种方式：
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix<float,2,3> matrix_23;
// Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，也就是三维向量
Eigen::Vector3d v_3d;
// 这是一样的写法
Eigen::Matrix<float,3,1> matrix_31;
// Matrix3d 实际上是 Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33;
// 矩阵初始化为零
matrix_33 = Eigen::Matrix3d::Zero();
// 如果不确定矩阵大小，可以使用动态大小的矩阵
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
// 更简单的方式
Eigen::MatrixXd matrix_x;数据初始化，可以直接用<<的方式赋值或访问：
// 输入数据（初始化）
matrix_23 << 1,4,8,3,6,1;
std::cout << "matrix_23:\n" << matrix_23 << std::endl;
// 用()访问矩阵中的元素
std::cout<<"用()访问矩阵中的元素"<<std::endl;
for (int i=0; i<2; i++) {
for (int j=0; j<3; j++)
std::cout<<matrix_23(i,j)<<"\t";
std::cout<<std::endl;
}不同向量或矩阵可能有类型上的不同，在Eigen里你不能混合两种不同类型的矩阵，这是错误示范：
Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
转换类型需要用显式转换，如下：
// 矩阵和向量相乘（实际上还是矩阵和矩阵）
v_3d << 3, 2, 1;
matrix_33 << 4, 5, 6;
// Eigen里的不同类型之间需要类型转换，要用显式转换的方式，如下：
Eigen::Matrix<double,2,1> result_1 = matrix_23.cast<double>() * v_3d;
std::cout<<"result1:\n"<<result_1<<std::endl;
Eigen::Matrix<float,2,1> result_2 = matrix_23 * matrix_31;
std::cout<<"result2:\n"<<result_2<<std::endl;基本运算举例：
// 一些简单的运算举例
Eigen::Matrix<double,3,3> Rmatrix_33;
Rmatrix_33 = Eigen::Matrix3d::Random();
std::cout << "Random Matrix:\n"<<Rmatrix_33<<std::endl;
std::cout << Rmatrix_33.transpose() << std::endl; // 转置
std::cout << Rmatrix_33.sum() << std::endl; // 各元素和
std::cout << Rmatrix_33.trace() << std::endl; // 迹
std::cout << 10*Rmatrix_33 << std::endl; // 数乘
std::cout << Rmatrix_33+Rmatrix_33 << std::endl; // 加减
std::cout << Rmatrix_33.inverse() << std::endl; // 逆
std::cout << Rmatrix_33.determinant() << std::endl; // 行列式矩阵特征值计算：
// 特征值
// Eigen::SelfAdjointEigenSolver
// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
std::cout << "Eigen values = \n" << eigen_solver.eigenvalues() << std::endl;
std::cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << std::endl;
解方程：
// 解方程
// 求解 matrix_NN * x = v_Nd 方程
// N的大小在前边的宏里定义，它由随机数生成
// 直接求逆自然是最直接的，但是求逆运算量大
Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);求逆：
clock_t time_stt = clock(); // 计时用
// 直接求逆 inverse
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
std::cout <<"time use in normal inverse is: " << 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms"<< std::endl;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
// colPivHouseholderQr
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
std::cout <<"time use in Qr decomposition is: " <<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC <<"ms" << std::endl;Cmake
Cmake是（跨平台的）安装编译工具， 可以用简单的语句来描述所有平台的安装(编译过程) 。本篇只先介绍简单的cmake语法。
既然是面向编译，就需要有程序用来编译。以下是举例的三个c++程序：
// hello.cpp
#include <iostream>
using namespace std;
int main( int argc, char** argv )
{
cout<<"Hello Pokemon!"<<endl;
return 0;
}以及可以用库的方式：
// libHello.cpp
// 这是一个库文件
#include <iostream>
using namespace std;
void printHello()
{
cout<<"Hello SLAM"<<endl;
}对应头文件：
// libHello.h
#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
// 上面的宏定义是为了防止重复引用这个头文件而引起的重定义错误
void printHello();
#endif使用这个库文件：
// useHello.cpp
#include "libHello.h"
// 使用 libHelloSLAM.h 中的 printHello() 函数
int main( int argc, char** argv )
{
printHello();
return 0;
}有了上述程序，现在来写CMakeLists
# 声明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )
# 声明一个 cmake 工程
project( Hello )
# 设置编译模式
set( CMAKE_BUILD_TYPE "Debug" )
# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable( hello hello.cpp )
# 添加一个库
add_library( libhello libHello.cpp )
# 共享库
add_library( libhello_shared SHARED libHello.cpp )
add_executable( useHello useHello.cpp )
# 将库文件链接到可执行程序上
target_link_libraries( useHello libhello_shared )编译的时候，建议参考以下步骤，将源码和编译文件区分开来：
mkdir build && cd bulid
cmake ..
make本代码生成的文件目录如下：
CMakeCache.txt CMakeFiles cmake_install.cmake hello liblibhello.a liblibhello_shared.so Makefile useHello运行效果请自行动手验证
现在来编写Eigen代码所需的CMakeLists：
# 声明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )
# 声明一个 cmake 工程
project( Eigen_learn )
# 设置编译模式
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable( Eigen_learn EigenMatrix.cpp )其中set( CMAKE_CXX_FLAGS "-O3" )是指定了g++，并选择了优化选项。
O0选项不进行任何优化，在这种情况下，编译器尽量的缩短编译消耗（时间，空间），此时，debug会产出和程序预期的结果。当程序运行被断点打断，此时程序内的各种声明是独立的，我们可以任意的给变量赋值，或者在函数体内把程序计数器指到其他语句,以及从源程序中 精确地获取你期待的结果.
O1优化会消耗少多的编译时间，它主要对代码的分支，常量以及表达式等进行优化。
O2会尝试更多的寄存器级的优化以及指令级的优化，它会在编译期间占用更多的内存和编译时间。
O3在O2的基础上进行更多的优化，例如使用伪寄存器网络，普通函数的内联，以及针对循环的更多优化。
Eigen几何模块
这里直接上代码，一些要点整理在注释了：
#include <iostream>
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
int main(int argc,char** argv)
{
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
// Identity() 初始一个单位矩阵
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
// 沿 Z 轴旋转 45 度
// #define M_PI 3.14159265358979323846
Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d ( 0,0,1 ));
// cout的一个成员函数，规定小数后保留几位，如3位的写法如下：
cout.precision(3);
// 用.matrix()转换成矩阵
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
cout<<"toRotationMatrix =\n"<< rotation_matrix <<endl;
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v (1,0,0);
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
// ZYX顺序，即roll pitch yaw顺序
// pitch是围绕X轴旋转，俯仰角
// yaw 是围绕Y轴旋转，偏航角
// roll 是围绕Z轴旋转，翻滚角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
// 欧氏变换矩阵使用 Eigen::Isometry
// 对于仿射和射影变换，使用 Eigen::Affine3d、Eigen::Projective3d
// 虽然称为3d，实质上是4＊4的矩阵
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
// 按照rotation_vector进行旋转
T.rotate ( rotation_vector );
// 把平移向量设成(1,3,4)
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );
cout << "Transform matrix = \n" << T.matrix() <<endl;
// 用变换矩阵进行坐标变换
// 相当于R*v+t
Eigen::Vector3d v_transformed = T*v;
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
// 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
// 注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<< q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v; // 注意数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
return 0;
} 对应的cmake为：
cmake_minimum_required( VERSION 2.8 )
project( Eigen_Geo_learn )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenGeometry EigneGeometry.cpp )Pangolin
参考如下代码，注意看其中的注释：
#include <pangolin/pangolin.h>
int main(int argc, char** argv)
{
// 创建名称为“Main”的GUI窗口，尺寸为640×640
pangolin::CreateWindowAndBind("Main",640,480);
// 启动深度测试
glEnable(GL_DEPTH_TEST);
// 创建一个观察相机视图
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(640,480,420,420,320,320,0.2,100),
pangolin::ModelViewLookAt(2,0,2, 0,0,0, pangolin::AxisY)
);
// 创建交互视图
pangolin::Handler3D handler(s_cam); //交互相机视图句柄
pangolin::View& d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
.SetHandler(&handler);
while(!pangolin::ShouldQuit())
{
// 清空颜色和深度缓存
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
// 在原点绘制一个立方体
pangolin::glDrawColouredCube();
// 绘制坐标系
glLineWidth(3);
glBegin ( GL_LINES );
glColor3f ( 0.8f,0.f,0.f );
glVertex3f( -1,-1,-1 );
glVertex3f( 0,-1,-1 );
glColor3f( 0.f,0.8f,0.f);
glVertex3f( -1,-1,-1 );
glVertex3f( -1,0,-1 );
glColor3f( 0.2f,0.2f,1.f);
glVertex3f( -1,-1,-1 );
glVertex3f( -1,-1,0 );
glEnd();
// 运行帧循环以推进窗口事件
pangolin::FinishFrame();
}
return 0;
}所需的CMakeLists为：
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(HelloPangolin main.cpp)
target_link_libraries(HelloPangolin ${Pangolin_LIBRARIES})最终效果图中，可以通过拖动画面与按动鼠标滚轮的方式实现基本的平移旋转功能
接下来参考如下代码，把物体旋转与平移的描述也展示出来了，以下是头文件部分：
#include <iostream>
#include <iomanip>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
#include <pangolin/pangolin.h>为了计算旋转矩阵、选转向量、四元数，里面重载过了运算符。主要是为了便于书写与ui展示，其展示效果可以看文末的插图。为了方便，这里把重载的部分单独拿出来。
struct RotationMatrix
{
Matrix3d matrix = Matrix3d::Identity();
};
ostream& operator << ( ostream& out, const RotationMatrix& r )
{
out.setf(ios::fixed);
Matrix3d matrix = r.matrix;
out<<'=';
out<<"["<<setprecision(2)<<matrix(0,0)<<","<<matrix(0,1)<<","<<matrix(0,2)<<"],"
<< "["<<matrix(1,0)<<","<<matrix(1,1)<<","<<matrix(1,2)<<"],"
<< "["<<matrix(2,0)<<","<<matrix(2,1)<<","<<matrix(2,2)<<"]";
return out;
}
istream& operator >> (istream& in, RotationMatrix& r )
{
return in;
}
struct TranslationVector
{
Vector3d trans = Vector3d(0,0,0);
};
ostream& operator << (ostream& out, const TranslationVector& t)
{
out<<"=["<<t.trans(0)<<','<<t.trans(1)<<','<<t.trans(2)<<"]";
return out;
}
istream& operator >> ( istream& in, TranslationVector& t)
{
return in;
}
struct QuaternionDraw
{
Quaterniond q;
};
ostream& operator << (ostream& out, const QuaternionDraw quat )
{
auto c = quat.q.coeffs();
out<<"=["<<c[0]<<","<<c[1]<<","<<c[2]<<","<<c[3]<<"]";
return out;
}
istream& operator >> (istream& in, const QuaternionDraw quat)
{
return in;
}主函数
int main ( int argc, char** argv )
{
// 创建名称为“visualize geometry”的GUI窗口，尺寸为1000×600
pangolin::CreateWindowAndBind ( "visualize geometry", 1000, 600 );
// 启动深度测试
glEnable ( GL_DEPTH_TEST );
// 创建一个观察相机视图
pangolin::OpenGlRenderState s_cam (
pangolin::ProjectionMatrix ( 1000, 600, 420, 420, 500, 300, 0.1, 1000 ),
pangolin::ModelViewLookAt ( 3,3,3,0,0,0,pangolin::AxisY )
);
const int UI_WIDTH = 500;
// 创建交互视图
pangolin::View& d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f/600.0f)
.SetHandler(new pangolin::Handler3D(s_cam));// SetHandler,交互相机视图句柄
// ui,设置需要显示的值
pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
pangolin::CreatePanel("ui")
.SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
while ( !pangolin::ShouldQuit() )
{
// 清空颜色和深度缓存
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
d_cam.Activate( s_cam );
pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
Matrix<double,4,4> m = matrix;
// m = m.inverse();
RotationMatrix R;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
R.matrix(i,j) = m(j,i);
rotation_matrix = R;
TranslationVector t;
t.trans = Vector3d(m(0,3), m(1,3), m(2,3));
t.trans = -R.matrix*t.trans;
translation_vector = t;
TranslationVector euler;
euler.trans = R.matrix.transpose().eulerAngles(2,1,0);
euler_angles = euler;
QuaternionDraw quat;
quat.q = Quaterniond(R.matrix);
quaternion = quat;
glColor3f(1.0,1.0,1.0);
// 在原点绘制一个立方体
pangolin::glDrawColouredCube();
// 绘制坐标系
glLineWidth(3);
glColor3f ( 0.8f,0.f,0.f );
glBegin ( GL_LINES );
glVertex3f( 0,0,0 );
glVertex3f( 10,0,0 );
glColor3f( 0.f,0.8f,0.f);
glVertex3f( 0,0,0 );
glVertex3f( 0,10,0 );
glColor3f( 0.2f,0.2f,1.f);
glVertex3f( 0,0,0 );
glVertex3f( 0,0,10 );
glEnd();
// 运行帧循环以推进窗口事件
pangolin::FinishFrame();
}
}
对应的cmake文件
cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )
set(CMAKE_CXX_FLAGS "-std=c++11")
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
# 添加Pangolin依赖
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable( visualizeGeometry visualizeGeometry.cpp )
target_link_libraries( visualizeGeometry ${Pangolin_LIBRARIES} )运行效果
