标签：poses 1.0 -- 0.0 pangolin Ow GL 逐行

参考文章1
参考文章2

源码+逐行注释

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

//trajectory文件的路径（可以使用相对路径，也可以使用绝对路径）
string trajectory_file = "../trajectory.txt";  

//画轨迹函数的声明
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;//容器的方式存储
    //因为类型是Eigen的Isometry3d和cpp的类内存分配不一样，所以要指定内存的分配方式
    //即:Eigen::aligned_allocator< Isometry3d > 
    
    ifstream fin(trajectory_file); //读文件
    //未成功读取
    if (!fin) {  
        cout << "cannot find trajectory file at " << trajectory_file << endl;
        return 1;
    }
    //一行一行读取，直到文件尾标志（efo标志）
    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;

    // 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);
	/***
	设置颜色RGBA混合因子,前面参数表示源因子，后面参数表示目标因子
	GL_ZERO：表示使用0.0作为权重因子
	GL_ONE ： 表示使用1.0作为权重因子
	GL_SRC_ALPHA ：表示使用源颜色的alpha值来作为权重因子
	GL_DST_ALPHA ：表示使用目标颜色的alpha值来作为权重因子
	GL_ONE_MINUS_SRC_ALPHA： 表示用1.0-源颜色的alpha的值来作为权重因子
	GL_ONE_MINUS_DST_ALPHA： 表示用1.0-目标颜色的alpha的值来作为权重因子
	GL_SRC_COLOR>：表示用源颜色的四个分量作为权重因子
	在画图的过程中如果程序glClearColor()；glColor3f()则后者为源颜色，前者的颜色为目标颜色以上的GL_SRC_ALPHA和GL_ONE_MINUS_SRC_ALPHA是最常用的混合模式 ***/

	//创建相机视图
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

	//ProjectionMatrix()设置相机内参,参数分别表示相机分辨率(2)，焦距(1)，相机光心(3)，最远最大距离(2)
	//ModelViewLookAt()设置观看视角,上文对应的意思是在世界坐标(0，-0.1，-1.8)处观看坐标原点（0,0,0）并设置Y轴向上
	/**  另一种解释
	 定义观测方位向量：观测点位置：(mViewpointX mViewpointY mViewpointZ)
                     观测目标位置：(0, 0, 0)
		             观测的方位向量：(0.0,-1.0, 0.0)**/

    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));
	/**创建交互视图view,用于显示上一步摄像机所“拍摄”到的内容，,setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右）,显示界面长宽比*/

    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);//为颜色缓存区指定确定的值
        glLineWidth(2);
        for (size_t i = 0; i < poses.size(); i++) {
            // 画每个位姿的三个坐标轴
            Vector3d Ow = poses[i].translation();//无参数，返回当前变换平移部分的向量表示(可修改)，可以索引[]获取各分量
            //对三个坐标轴分别旋转
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));//0.1是为了让图看起来舒服，不会太大
            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);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[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
    }
}

运行效果截图

欢迎SLAMers加QQ：258404070 一起学习SLAM
有问题请留言

标签：poses,1.0,--,0.0,pangolin,Ow,GL,逐行
来源： https://blog.csdn.net/weixin_42126210/article/details/123031204

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。