# 點(diǎn)云示例-PointCloud

功能描述：連接設(shè)備開流 ,生成深度點(diǎn)云或RGBD點(diǎn)云并保存成ply格式文件，并通過ESC_KEY鍵退出程序

> 本示例基于C++ High Level API進(jìn)行演示

創(chuàng)建點(diǎn)云保存成ply格式文件函數(shù)，ply文件格式詳細(xì)描述可在網(wǎng)絡(luò)上查看<br />首先創(chuàng)建兩個(gè)函數(shù)來保存從流里面獲取到的點(diǎn)云數(shù)據(jù)，這是保存普通點(diǎn)云數(shù)據(jù)的函數(shù)

//保存點(diǎn)云數(shù)據(jù)到ply
void savePointsToPly(std::shared_ptr<ob::Frame> frame, std::string fileName) {
    int   pointsSize = frame->dataSize() / sizeof(OBPoint);
    FILE *fp         = fopen(fileName.c_str(), "wb+");
    fprintf(fp, "ply\n");
    fprintf(fp, "format ascii 1.0\n");
    fprintf(fp, "element vertex %d\n", pointsSize);
    fprintf(fp, "property float x\n");
    fprintf(fp, "property float y\n");
    fprintf(fp, "property float z\n");
    fprintf(fp, "end_header\n");

    OBPoint *point = (OBPoint *)frame->data();
    for(int i = 0; i < pointsSize; i++) {
        fprintf(fp, "%.3f %.3f %.3f\n", point->x, point->y, point->z);
        point++;
    }

    fflush(fp);
    fclose(fp);
}

再創(chuàng)建一個(gè)保存彩色點(diǎn)云數(shù)據(jù)的函數(shù)用于存儲(chǔ)彩色點(diǎn)云數(shù)據(jù)

//保存彩色點(diǎn)云數(shù)據(jù)到ply
void saveRGBPointsToPly(std::shared_ptr<ob::Frame> frame, std::string fileName) {
    int   pointsSize = frame->dataSize() / sizeof(OBColorPoint);
    FILE *fp         = fopen(fileName.c_str(), "wb+");
    fprintf(fp, "ply\n");
    fprintf(fp, "format ascii 1.0\n");
    fprintf(fp, "element vertex %d\n", pointsSize);
    fprintf(fp, "property float x\n");
    fprintf(fp, "property float y\n");
    fprintf(fp, "property float z\n");
    fprintf(fp, "property uchar red\n");
    fprintf(fp, "property uchar green\n");
    fprintf(fp, "property uchar blue\n");
    fprintf(fp, "end_header\n");

    OBColorPoint *point = (OBColorPoint *)frame->data();
    for(int i = 0; i < pointsSize; i++) {
        fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", point->x, point->y, point->z, (int)point->r, (int)point->g, (int)point->b);
        point++;
    }

    fflush(fp);
    fclose(fp);
}

設(shè)置Log等級(jí)，避免過多Info等級(jí)的Log影響點(diǎn)云輸出的結(jié)果

ob::Context::setLoggerSeverity(OB_LOG_SEVERITY_ERROR);

創(chuàng)建一個(gè)Pipeline，通過Pipeline可以很容易的打開和關(guān)閉多種類型的流并獲取一組幀數(shù)據(jù)

ob::Pipeline pipeline;

配置color流

auto colorProfiles = pipeline.getStreamProfileList(OB_SENSOR_COLOR);
if(colorProfiles) {
    auto profile = colorProfiles->getProfile(OB_PROFILE_DEFAULT);
    colorProfile = profile->as<ob::VideoStreamProfile>();
}
config->enableStream(colorProfile);

配置深度流

std::shared_ptr<ob::StreamProfileList> depthProfileList;
OBAlignMode                            alignMode = ALIGN_DISABLE;
if(colorProfile) {
    // Try find supported depth to color align hardware mode profile
    depthProfileList = pipeline.getD2CDepthProfileList(colorProfile, ALIGN_D2C_HW_MODE);
    if(depthProfileList->count() > 0) {
        alignMode = ALIGN_D2C_HW_MODE;
    }
    else {
        // Try find supported depth to color align software mode profile
        depthProfileList = pipeline.getD2CDepthProfileList(colorProfile, ALIGN_D2C_SW_MODE);
        if(depthProfileList->count() > 0) {
            alignMode = ALIGN_D2C_SW_MODE;
        }
    }
}
else {
    depthProfileList = pipeline.getStreamProfileList(OB_SENSOR_DEPTH);
}

if(depthProfileList->count() > 0) {
    std::shared_ptr<ob::StreamProfile> depthProfile;
    try {
        // Select the profile with the same frame rate as color.
        if(colorProfile) {
            depthProfile = depthProfileList->getVideoStreamProfile(OB_WIDTH_ANY, OB_HEIGHT_ANY, OB_FORMAT_ANY, colorProfile->fps());
        }
    }
    catch(...) {
        depthProfile = nullptr;
    }

    if(!depthProfile) {
        // If no matching profile is found, select the default profile.
        depthProfile = depthProfileList->getProfile(OB_PROFILE_DEFAULT);
    }
    config->enableStream(depthProfile);

開啟D2C對(duì)齊, 生成RGBD點(diǎn)云時(shí)需要開啟

// 開啟D2C對(duì)齊, 生成RGBD點(diǎn)云時(shí)需要開啟
config->setAlignMode(ALIGN_D2C_HW_MODE);

啟動(dòng)Pipeline

pipeline.start( config );

創(chuàng)建點(diǎn)云Filter對(duì)象，并且設(shè)置相機(jī)內(nèi)參

// 創(chuàng)建點(diǎn)云Filter對(duì)象（點(diǎn)云Filter創(chuàng)建時(shí)會(huì)在Pipeline內(nèi)部獲取設(shè)備參數(shù), 所以盡量在Filter創(chuàng)建前配置好設(shè)備）
ob::PointCloudFilter pointCloud;

//獲取相機(jī)內(nèi)參傳入點(diǎn)云Filter中
auto cameraParam = pipeline.getCameraParam();
pointCloud.setCameraParam(cameraParam)

設(shè)置些操作提示

 std::cout << "Press R to create rgbd pointCloud and save to ply file! " << std::endl;
 std::cout << "Press d to create depth pointCloud and save to ply file! " << std::endl;
 std::cout << "Press ESC to exit! " << std::endl;

設(shè)置主流程通過上面創(chuàng)建的點(diǎn)云Filter對(duì)象獲取并保存點(diǎn)云數(shù)據(jù)

if(key == 'R' || key == 'r') {
  count = 0;
  //限制最多重復(fù)10次
  while(count++ < 10) {
    //等待一幀數(shù)據(jù)，超時(shí)時(shí)間為100ms
    auto frameset = pipeline.waitForFrames(100);
    if(frameset != nullptr && frameset->depthFrame() != nullptr && frameset->colorFrame() != nullptr) {
      try {
        //生成彩色點(diǎn)云并保存
        std::cout << "Save RGBD PointCloud ply file..." << std::endl;
        pointCloud.setCreatePointFormat(OB_FORMAT_RGB_POINT);
        std::shared_ptr<ob::Frame> frame = pointCloud.process(frameset);
        saveRGBPointsToPly(frame, "RGBPoints.ply");
        std::cout << "RGBPoints.ply Saved" << std::endl;
      }
      catch(std::exception &e) {
        std::cout << "Get point cloud failed" << std::endl;
      }
      break;
    }
  }
}
else if(key == 'D' || key == 'd') {
  count = 0;
  //限制最多重復(fù)10次
  while(count++ < 10) {
    //等待一幀數(shù)據(jù)，超時(shí)時(shí)間為100ms
    auto frameset = pipeline.waitForFrames(100);
    if(frameset != nullptr && frameset->depthFrame() != nullptr) {
      try {
        //生成點(diǎn)云并保存
        std::cout << "Save Depth PointCloud to ply file..." << std::endl;
        pointCloud.setCreatePointFormat(OB_FORMAT_POINT);
        std::shared_ptr<ob::Frame> frame = pointCloud.process(frameset);
        savePointsToPly(frame, "DepthPoints.ply");
        std::cout << "DepthPoints.ply Saved" << std::endl;
      }
      catch(std::exception &e) {
        std::cout << "Get point cloud failed" << std::endl;
      }
      break;
    }
  }
}

最后通過Pipeline來停止流

 pipeline.stop();

程序正常退出后會(huì)釋放資源

預(yù)期輸出：