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