pangolin于SLAM的实战学习（三）

目录

9.绘制点和制作点云效果

10.将图像特征点加入原第八程序

---

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

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

---

9.绘制点和制作点云效果

绘制点是比较简单的：

在绘图的子函数中加入类似如下语句即可，给予颜色和位置。

for(int p=0;p<pose_of_world.size();p++)
{
    glColor3d(pose_of_world[p][5]/255,pose_of_world[p][4]/255,pose_of_world[p][3]/255);
    glVertex3d(pose_of_world[p][0],pose_of_world[p][1],pose_of_world[p][2]);
}
    glEnd();

绘制点云还需要原始点的信息。类似于SLAM14讲的第五讲拼接点云。

cv::Mat color = colorImgs[i]; 
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];	
for ( int v=0; v<color.rows; v++ )
{
    uchar* data = color.ptr<uchar>(v);
    for ( int u=0; u<color.cols; u++ )
    {
        unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
        if ( d==0 ) continue; // 为0表示没有测量到
        Eigen::Vector3d point; vector<double> point_pose;
        point[2] = double(d)/depthScale; 
        point[0] = (u-cx)*point[2]/fx;
        point[1] = (v-cy)*point[2]/fy; 
        Eigen::Vector3d pointWorld = T*point;
	point_pose.push_back(pointWorld[0]);
	point_pose.push_back(pointWorld[1]);
	point_pose.push_back(pointWorld[2]);
	point_pose.push_back(double(data[3*u+0]));
	point_pose.push_back(double(data[3*u+1]));
	point_pose.push_back(double(data[3*u+2]));
	pose_of_world.push_back(point_pose);
	point_pose.clear();
    } 
}

效果如图：

10.将图像特征点加入原第八程序

大概效果这样：

要修改的地方有获取特征点位置与颜色：

feature_img = color.clone();
cv::Ptr<cv::FeatureDetector> detector;
detector = ORB::create();
vector< cv::KeyPoint > kp1;
detector->detect( color, kp1 ); 
for(int kp_num=0;kp_num<kp1.size();kp_num++)
{
    vector<double> point_pose;
    Eigen::Vector3d point;
    // 深度值
    unsigned int d = depth.ptr<unsigned short>(int ( kp1[kp_num].pt.y ))[int(kp1[kp_num].pt.x)]; 
    point[2] = double(d)/depthScale; 
    point[0] = (int (kp1[kp_num].pt.x)-cx)*point[2]/fx;
    point[1] = (int (kp1[kp_num].pt.y)-cy)*point[2]/fy; 
    Eigen::Vector3d pointWorld = T*point;
    point_pose.push_back(pointWorld[0]);
    point_pose.push_back(pointWorld[1]);
    point_pose.push_back(pointWorld[2]);
    point_pose.push_back(double(color.at<Vec3b>(int ( kp1[kp_num].pt.y ),int ( kp1[kp_num].pt.x ))[0]));
    point_pose.push_back(double(color.at<Vec3b>(int ( kp1[kp_num].pt.y ),int ( kp1[kp_num].pt.x ))[1]));
    point_pose.push_back(double(color.at<Vec3b>(int ( kp1[kp_num].pt.y ),int ( kp1[kp_num].pt.x ))[2]));
    pose_of_world.push_back(point_pose);
    point_pose.clear();
}

绘制特征点图：

if(int ( kp1[kp_num].pt.y)-1>0&int(kp1[kp_num].pt.y)-1<640&int(kp1[kp_num].pt.y)-1>0&int(kp1[kp_num].pt.y)-1<640)
{
for(int fea_dx=-1;fea_dx<2;fea_dx++)
{
    for(int fea_dy=-1;fea_dy<2;fea_dy++)
    {
	  feature_img.at<Vec3b>(int ( kp1[kp_num].pt.y+fea_dx ),int ( kp1[kp_num].pt.x+fea_dy ))[0]=0;
	  feature_img.at<Vec3b>(int ( kp1[kp_num].pt.y+fea_dx ),int ( kp1[kp_num].pt.x+fea_dy ))[1]=255;
	  feature_img.at<Vec3b>(int ( kp1[kp_num].pt.y+fea_dx ),int ( kp1[kp_num].pt.x+fea_dy ))[2]=0;
    }
}
}

绘制地图点：

//画点
glPointSize(2);
glBegin(GL_POINTS);
for(int p=0;p<pose_of_world.size();p++)
{
    glColor3d(pose_of_world[p][5]/255,pose_of_world[p][4]/255,pose_of_world[p][3]/255);
    glVertex3d(pose_of_world[p][0],pose_of_world[p][1],pose_of_world[p][2]);
}
    glEnd();

修改GUI：

//设计显示面板
pangolin::CreatePanel("menu").SetBounds(0.33,1,0.0,0.3);

//定义图片面板
pangolin::View& fea_image = pangolin::Display("feature") 
.SetBounds(0,0.5,0.0,0.33,1024.0f/768.0f)
.SetLock(pangolin::LockLeft, pangolin::LockBottom);

pangolin::View& rgb_image = pangolin::Display("rgb") 
.SetBounds(0,0.5,0.33,0.66,1024.0f/768.0f)
.SetLock(pangolin::LockLeft, pangolin::LockBottom);

pangolin::View& depth_image = pangolin::Display("depth")
.SetBounds(0,0.5,0.66,0.99,1024.0f/768.0f)
.SetLock(pangolin::LockLeft, pangolin::LockBottom);