octomap包的安裝與編譯


這里需要兩個包,一個是octomap的包,一個是放了怎么做這種圖的程序包。一定要把這兩個包放在同一目錄下,不要放在catkin工作空間,因為octomap這個包會使catkin工作空間catkin_make失敗。
1.編譯
安裝依賴
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev
 

git clone https://github.com/OctoMap/octomap

下面就是對這個包進行編譯,就是'
mkdir build
cd build
cmake ..
make
只所以新建一個build文件夾,是因為要在里面cmake的話,會把編譯過程中產生的緩存文件放在這個目錄,如果直接cmake .,會不太好整理文件。就像我每次重新編譯的時候,直接把build文件下的所以東西都刪掉就可以了。
2.查看示例地圖

bin/octovis octomap/share/data/geb079.bt

大概長這樣



這是一層樓的掃描圖。
3.把點雲地圖轉換成octomap
git clone https://github.com/gaoxiang12/octomap_tutor
編譯這個包,依然是
mkdir build
cd build
cmake ..
make
編譯之后會在bin目錄下生成幾個可執行程序。這個例子里的程序對應的是pcd2octomap源代碼在src/pcd2octomap.cpp里:
#include <iostream>
#include <assert.h>
//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//octomap 
#include <octomap/octomap.h>
using namespace std;
int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl;
        return -1;
    }
    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );
    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;
    //聲明octomap變量
    cout<<"copy data into octomap..."<<endl;
    // 創建八叉樹對象,參數為分辨率,這里設成了0.05
    octomap::OcTree tree( 0.05 );
    for (auto p:cloud.points)
    {
        // 將點雲里的點插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }
    // 更新octomap
    tree.updateInnerOccupancy();
    // 存儲octomap
    tree.writeBinary( output_file );
    cout<<"done."<<endl;
    return 0;
}
這個代碼也比較簡單.用的時候可以直接調用
bin/pcd2octomap data/sample.pcd data/sample.bt
這個程序的作用是把data下的點雲數據sample.pcd轉換成octovis格式的數據(它有兩種格式一個是bt,一個是ot,其它格式octovis識別不了),生成之后的文件是sample.bt,
比較結果
pcl_viewer data/sample.pcd
../octomap-devel/bin/octovis data/sample.bt
 
4.加入色彩信息
點雲是有色彩信息的,octomap也有色彩信息,是用ColorOcTree類來存儲色彩信息.用的程序是/bin/pcd2colorOctomap,源代碼在src目錄下。
#include <iostream>
#include <assert.h>
//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
using namespace std;
int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2colorOctomap <input_file> <output_file>"<<endl;
        return -1;
    }
    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );
    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;
    //聲明octomap變量
    cout<<"copy data into octomap..."<<endl;
    // 創建帶顏色的八叉樹對象,參數為分辨率,這里設成了0.05
    octomap::ColorOcTree tree( 0.05 );
    for (auto p:cloud.points)
    {
        // 將點雲里的點插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }
    // 設置顏色
    for (auto p:cloud.points)
    {
        tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }
    // 更新octomap
    tree.updateInnerOccupancy();
    // 存儲octomap, 注意要存成.ot文件而非.bt文件
    tree.write( output_file );
    cout<<"done."<<endl;
    return 0;
}
這個程序是對之前的pcd2octomap做了一些修改,比如OcTree改成ColorOcTree,以及調用integrateNodeColor來混合顏色等。
bin/pcd2colorOctomap data/sample.pcd data/sample.ot
 ../octomap-devel/bin/octovis data/sample.ot 
效果如下
 
 

5.拼接與轉換

這里做的是把五張RGBD圖拼接成octomap,程序是joinMap,源代碼如下:
 
#include <iostream>
#include <vector>
 
// octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h>
 
// opencv 用於圖像數據讀取與處理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
 
// 使用Eigen的Geometry模塊處理3d運動
#include <Eigen/Core>
#include <Eigen/Geometry> 
 
// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
 
// boost.format 字符串處理
#include <boost/format.hpp>
 
using namespace std;
 
// 全局變量:相機矩陣
// 更好的寫法是存到參數文件中,但為方便起見我就直接這樣做了
float camera_scale  = 1000;
float camera_cx     = 325.5;
float camera_cy     = 253.5;
float camera_fx     = 518.0;
float camera_fy     = 519.0;
 
int main( int argc, char** argv )
{
    // 讀關鍵幀編號
    ifstream fin( "./data/keyframe.txt" );
    vector<int> keyframes;
    vector< Eigen::Isometry3d > poses;
    // 把文件 ./data/keyframe.txt 里的數據讀取到vector中
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        fin>>index_keyframe;
        if (fin.fail()) break;
        keyframes.push_back( index_keyframe );
    }
    fin.close();
 
    cout<<"load total "<<keyframes.size()<<" keyframes. "<<endl;
 
    // 讀關鍵幀姿態
    // 我的代碼中使用了Eigen來存儲姿態,類似的,也可以用octomath::Pose6D來做這件事
    fin.open( "./data/trajectory.txt" );
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        float data[7]; // 三個位置加一個姿態四元數x,y,z, w,ux,uy,uz
        fin>>index_keyframe;
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
            cout<<data[i]<<" ";
        }
        cout<<endl;
        if (fin.fail()) break;
        // 注意這里的順序。g2o文件四元數按 qx, qy, qz, qw來存
        // 但Eigen初始化按照qw, qx, qy, qz來做
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d t(q);
        t(0,3) = data[0]; t(1,3) = data[1]; t(2,3) = data[2];
        poses.push_back( t );
    }
    fin.close();
 
    // 拼合全局地圖
    octomap::ColorOcTree tree( 0.05 ); //全局map
 
    // 注意我們的做法是先把圖像轉換至pcl的點雲,進行姿態變換,最后存儲成octomap
    // 因為octomap的顏色信息不是特別方便處理,所以采用了這種迂回的方式
    // 所以,如果不考慮顏色,那不必轉成pcl點雲,而可以直接使用octomap::Pointcloud結構
 
    for ( size_t i=0; i<keyframes.size(); i++ )
    {
        pcl::PointCloud<pcl::PointXYZRGBA> cloud; 
        cout<<"converting "<<i<<"th keyframe ..." <<endl;
        int k = keyframes[i];
        Eigen::Isometry3d& pose = poses[i];
 
        // 生成第k幀的點雲,拼接至全局octomap上
        boost::format fmt ("./data/rgb_index/%d.ppm" );
        cv::Mat rgb = cv::imread( (fmt % k).str().c_str() );
        fmt = boost::format("./data/dep_index/%d.pgm" );
        cv::Mat depth = cv::imread( (fmt % k).str().c_str(), -1 );
 
        // 從rgb, depth生成點雲,運算方法見《一起做》第二講
        // 第一次遍歷用於生成空間點雲
        for ( int m=0; m<depth.rows; m++ )
            for ( int n=0; n<depth.cols; n++ )
            {
                ushort d = depth.ptr<ushort> (m) [n];
                if (d == 0)
                    continue;
                float z = float(d) / camera_scale;
                float x = (n - camera_cx) * z / camera_fx;
                float y = (m - camera_cy) * z / camera_fy;
                pcl::PointXYZRGBA p;
                p.x = x; p.y = y; p.z = z;
 
                uchar* rgbdata = &rgb.ptr<uchar>(m)[n*3];
                uchar b = rgbdata[0];
                uchar g = rgbdata[1];
                uchar r = rgbdata[2];
 
                p.r = r; p.g = g; p.b = b;
                cloud.points.push_back( p ); 
            }
        // 將cloud旋轉之后插入全局地圖
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp( new pcl::PointCloud<pcl::PointXYZRGBA>() );
        pcl::transformPointCloud( cloud, *temp, pose.matrix() );
 
        octomap::Pointcloud cloud_octo;
        for (auto p:temp->points)
            cloud_octo.push_back( p.x, p.y, p.z );
 
        tree.insertPointCloud( cloud_octo, 
                octomap::point3d( pose(0,3), pose(1,3), pose(2,3) ) );
 
        for (auto p:temp->points)
            tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }
 
    tree.updateInnerOccupancy();
    tree.write( "./data/map.ot" );
 
    cout<<"done."<<endl;
 
    return 0;
 
}
這個程序是把RGBD圖先轉換成點雲,再放到octotree,這樣做出來的圖是有顏色的。


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM