SLAM学习笔记——结合pangolin和eigen可视化

pangolin是一个基于opengl开发的图形库，可以在linux上运行。可以使用eigen和pangolin自己写一个程序展示运行轨迹和坐标变换，比起rviz+tf的拓展性更高。下面是注释的很详细的代码，main里面生成了一个椭圆的轨迹
code:

#include"ros/ros.h"
#include"Eigen/Core"
#include"Eigen/Geometry"
#include"Eigen/Dense"
#include"pangolin/pangolin.h"
#include <unistd.h>
using namespace std;
using namespace Eigen;
// path to trajectory file
string trajectory_file = "/home/jimazeyu/trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
	//从文件中读取
  // ifstream fin(trajectory_file);
  // if (!fin) {
  //   cout << "cannot find trajectory file at " << trajectory_file << endl;
  //   return 1;
  // }

  // while (!fin.eof()) {
  //   double time, tx, ty, tz, qx, qy, qz, qw;
  //   fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
  //   Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
  //   Twr.pretranslate(Vector3d(tx, ty, tz));
  //   poses.push_back(Twr);
  // }
  // cout << "read total " << poses.size() << " pose entries" << endl;

	//生成一个椭圆形轨道
	int a=2,b=3;
	//平移
	Vector3d t(0,0,0);
	//旋转
	AngleAxisd rotation(M_PI/4,Vector3d(1,1,1));
	Isometry3d trans=Isometry3d::Identity();
	trans.rotate(rotation);
	trans.pretranslate(t);
	
	for(int theta=0;theta<360;theta++)
	{
		double x=1.0*a*cos(1.0*theta/180*M_PI);
		double y=1.0*b*sin(1.0*theta/180*M_PI);
		Vector3d tmp_pos=trans*Vector3d(x,y,0);
		AngleAxisd tmp_rotation(0,tmp_pos);
		Isometry3d Twr(tmp_rotation);
		Twr.pretranslate(tmp_pos);
		poses.push_back(Twr);
	}
  // draw trajectory in pangolin
  DrawTrajectory(poses);
  return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST); //3d必开，只现实镜头视角的像素
  glEnable(GL_BLEND); //颜色混合，透过蓝玻璃看黄玻璃变绿
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //设置上一行的混合方式http://blog.chinaunix.net/uid-20622737-id-2850251.html

  //配置相机视角的参数
  pangolin::OpenGlRenderState s_cam(
    //ProjectMatrix(int h, int w, int fu, int fv, int cu, int cv, int znear, int zfar)是用来配置相机的内参，参数依次为相机的图像高度、宽度、4个内参以及最近和最远视距
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), 
    /*
    ModelViewLookAt(double x, double y, double z,double lx, double ly, double lz, AxisDirection Up)
    前三个参数依次为相机所在的位置，第四到第六个参数相机所看的视点位置(一般会设置在原点)，最后是
    相机轴的方向，最终在GUI中呈现的图像就是通过这个设置的相机内外参得到的。你可以用自己的脑袋当
    做例子，前三个参数告诉你脑袋在哪里，然后再告诉你看的东西在哪里，最后告诉你的头顶朝着哪里。
    */
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  /*
  进行显示设置。SetBounds函数前四个参数依次表示视图在视窗中的范围（下、上、左、右），最后一个参数是显示的长宽比。
  （0.0, 1.0, 0.0, 1.0）第一个参数0.0表示显示的拍摄窗口的下边在整个GUI中最下面，第二个参数1.0表示上边在GUI的最上面，以此类推。如果在中间就用0.5表示。
  */
  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));  //创建相机视图句柄

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存。这样每次都会刷新显示，不至于前后帧的颜信息相互干扰。
    d_cam.Activate(s_cam);//激活显示并设置状态矩阵
    //注释下面一行会使背景为黑色
    //glClearColor(1.0f, 1.0f, 1.0f, 1.0f); //前三个分别代表红、绿、蓝所占的分量，范围从0.0f~1.0f,最后一个参数是透明度Alpha值,范围也是0.0f~1.0f
    glLineWidth(2);//线宽
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
			//将自身坐标系下的三个轴变换到世界坐标系下，并且长度缩小为0.1
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES); //开始画线
      glColor3f(1.0, 0.0, 0.0);//颜色设置rgb
      glVertex3d(Ow[0], Ow[1], Ow[2]);//端点1
      glVertex3d(Xw[0], Xw[1], Xw[2]);//端点2
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();//结束画线
    }
    // 画出连线
		for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame(); //执行后期渲染，事件处理和帧交换，相当于前面设置了那么多现在可以进行最终的显示了。
    usleep(5000);   // sleep 5 ms
  }
}

cmakelist:

cmake_minimum_required(VERSION 3.0.2)
project(eigen_tutorials)

find_package(catkin REQUIRED COMPONENTS
  roscpp
)

find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)

catkin_package(
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
  ${Pangolin_INCLUDE_DIRS}
)
#add_executable(tf_visualization src/tf_visualization.cpp)

target_link_libraries(tf_visualization ${Pangolin_LIBRARIES})