pangolin于SLAM的实战学习（二）

目录

5.添加选项

6.添加按钮

7.添加文字输出

8.添加图片

---

程序：https://github.com/eminbogen/7.16Pangolin

上一节：https://blog.youkuaiyun.com/unlimitedai/article/details/96096868

下一节：https://blog.youkuaiyun.com/unlimitedai/article/details/96827752

---

5.添加选项

选项我设计为挂钩型（暂停功能和轨迹线是否显示功能）和数值型（地图显示变化速度调节）。变化如下：

//全局变量：
//给定初始速度和初始状态（运行）
int slam_speed=1;int command_go=1;

//主函数：
//利用序列更新时间长短变化改变更新地图速度
while(!command_go){}
usleep(1000000/slam_speed);

//构图函数：
//第一个参数为按钮的名字，第二个为默认状态，第三个为最低值，第四个为最高值
pangolin::Var<int> a_int("menu.slam_speed",2,1,10);
//第一个参数为按钮的名字，第二个为默认状态，第三个为是否有选择框
pangolin::Var<bool> menu("menu.lines",true,true);
pangolin::Var<bool> goon("menu.go_on",true,true);

//构图函数的循环：
//速度变更
slam_speed =a_int;
//运行状态变更
if(goon) command_go = 1;if(!goon) command_go = 0;

//绘制连接的绿色线，并根据选项决定是否绘制绿线
if(menu)
{
    glLineWidth(2);
    glBegin ( GL_LINES );
    glColor3f ( 0.0f,1.f,0.f );
    for(int i=0;i<pose_fin.size()-1;i++)
    {
	glVertex3f( pose_fin[i][9],pose_fin[i][10],pose_fin[i][11]);
	glVertex3f( pose_fin[i+1][9],pose_fin[i+1][10],pose_fin[i+1][11] );
    }
    glEnd();
}

 

6.添加按钮

主要是为了保存和中途退出。增加内容如下：

// 设置按钮
pangolin::Var<bool> save_window("menu.Save_Window",false,false);
pangolin::Var<bool> save_cube("menu.Save_Cube",false,false);
pangolin::Var<std::function<void(void)> > reset("menu.ESC");

//使用按钮
if( pangolin::Pushed(save_window) )
pangolin::SaveWindowOnRender("window");
if( pangolin::Pushed(save_cube) )
d_cam.SaveOnRender("cube");

7.添加文字输出

之后，作为一个界面，怎么着也得有个文本输出吧，so。。

首先需要辅助输出的结构体和子函数。为后面设计文本输出做准备。

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;
}

前面使用了结构体为变量，后面使用了函数作输出，中间定义的变量供后续使用。

结构体：RotationMatrix

中间量：rotation_matrix

子函数：RotationMatrix

//设计文本输出于面板
pangolin::Var<RotationMatrix> rotation_matrix("menu.r", RotationMatrix());
pangolin::Var<TranslationVector> translation_vector("menu.t", TranslationVector());
//初始化
pangolin::GlTexture imageTexture(640,480,GL_RGB,false,0,GL_BGR,GL_UNSIGNED_BYTE);

//运行R，t输出
if(pose_fin.size()>0)
{
    RotationMatrix R; 
    for (int i=0; i<3; i++)
	for (int j=0; j<3; j++)
	    R.matrix(j,i) = double(pose_fin[k-1][3*j+i]);
    rotation_matrix = R;
    
    TranslationVector t;
    t.trans = Vector3d(pose_fin[k-1][9],pose_fin[k-1][10],pose_fin[k-1][11]);
    t.trans = -R.matrix*t.trans;
    translation_vector = t;
}

当然，这里我修改了界面尺寸，地图模块的参数，自己添加练习的同学建议看一下我这里的程序。

8.添加图片

如果你下载好了数据集，那么后面可以进行了，当然，增加图片程序变化本身不大。

//定义图片面板
pangolin::View& rgb_image = pangolin::Display("rgb")
  .SetBounds(0,0.3,0.3,0.65,1024.0f/768.0f)
  .SetLock(pangolin::LockLeft, pangolin::LockBottom);

pangolin::View& depth_image = pangolin::Display("depth")
  .SetBounds(0,0.3,0.65,1,1024.0f/768.0f)
  .SetLock(pangolin::LockLeft, pangolin::LockBottom);

//图像读取，显示
cv::Mat rgb;
rgb = cv::imread (v_rgb.at(k-1));
imageTexture.Upload(rgb.data,GL_BGR,GL_UNSIGNED_BYTE);
rgb_image.Activate();
glColor3f(1.0,1.0,1.0);
imageTexture.RenderToViewportFlipY();

depth_image.Activate();
cv::Mat depth;
depth = cv::imread (v_depth.at(k-1));
imageTexture.Upload(depth.data,GL_BGR,GL_UNSIGNED_BYTE);
glColor3f(1.0,1.0,1.0);
imageTexture.RenderToViewportFlipY();

不过依靠associate.txt读取图片路径的程序也要写，改变在子函数：

int get_pose(string path_to_data,vector<vector<float>> &pose,int index,int interval,vector<string> &v_rgb,vector<string> &v_depth)
{
    //定义文件路径
    string path_to_pose = path_to_data + "/groundtruth.txt";
    string path_to_imagepath = path_to_data + "/associate.txt";
    
    //检测文件是否存在，1为位姿，2为图片
    ifstream fin1( path_to_pose );
    if ( !fin1 ) 
    {
        cerr<<"I cann't find groundtruth.txt!"<<endl;
	return 1;
    }
    
    ifstream fin2( path_to_imagepath );
    if ( !fin2 ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
	return 1;
    }
    
    //定义用于存储fin2中时间与路径的变量
    string rgb_path,depth_path;
    double rgb_time,depth_time;
    
    for ( int i=0; i<index; i++ )
    {
	double temp_pose;float temp[8];vector< float>pose_temp;int interval_temp = interval;
	while(interval_temp--) fin1>>temp_pose>>temp[1]>>temp[2]>>temp[3]>>temp[4]>>temp[5]>>temp[6]>>temp[7];
	pose_temp.push_back(temp[1]);pose_temp.push_back(temp[2]);pose_temp.push_back(temp[3]);
	pose_temp.push_back(temp[7]);pose_temp.push_back(temp[4]);pose_temp.push_back(temp[5]);pose_temp.push_back(temp[6]);
	pose.push_back(pose_temp);
	pose_temp.clear();
	
	//这是一个标志
	int flag_num_match = 1;
	while(flag_num_match)
	{
	       fin2>>rgb_time>>rgb_path>>depth_time>>depth_path;
	       //ta用于检测fin1读取位姿的时间和fin2读取的时间差距是否小于0.1s，小于则跳出死循环
	       if(abs(temp_pose-rgb_time)<0.1) flag_num_match=0;
	}
	//并把该时刻下的图片路径记录
	v_rgb.push_back(path_to_data+"/"+rgb_path);
	v_depth.push_back(path_to_data+"/"+depth_path);
    }
    return 1;
}

请看源码！