pangolin于SLAM的实战学习(二)

目录

5.添加选项

6.添加按钮

7.添加文字输出

8.添加图片


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

上一节:https://blog.csdn.net/unlimitedai/article/details/96096868

下一节:https://blog.csdn.net/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;
}

请看源码!

 

评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值