深度圖+點雲常用工具彙總

1. PCL (Point Cloud Library)

庫名 最小版本 描述
Eigen 3.0 高層次的C ++庫,有效支持線性代數,矩陣和矢量運算,數值分析及其相關的算法,功能列表:
Core:Matrix和Array類,基礎的線性代數運算和數組操作
Geometry:旋轉、平移、縮放、2D/3D旋轉(Quanternion, AngleAxis)
LU:求逆、行列式和LU分解
Cholesky:LLT和LDLT Cholesky分解
SVD:SVD分解(JacobiSVD)
QR:QR分解
Eigenvalues:求特徵值和特徵向量
FLANN 1.7.1 Fast Library for Approximate Nearest Neighbors (近似近鄰快速庫)
一個在高維空間中執行快速近似近鄰搜索的庫
最適合最近鄰搜索的算法,以及一個根據數據集自動選擇最佳算法和最佳參數的系統
Boost 1.40 (without OpenNI)
1.47 (with OpenNI)
爲C++語言標準庫提供擴展的一些C++程序庫的總稱
Boost庫是一個可移植、提供源代碼的C++庫,作爲標準庫的後備,是C++標準化進程的開發引擎之一
現有的 Boost 包含大約150種不同的函數庫
VTK 5.6 PCL使用的可視化工具 (Visualization Toolkit)
支持3D渲染、3D交互
  • PCL編譯
# flann庫
sudo apt-get install libflann1.8 libflann-dev 
# eigen庫
sudo apt-get install libeigen3-dev
# boost庫  
sudo apt-get install libboost-all-dev 
# VTK 
sudo apt-get install libvtk5.10-qt4 libvtk5.10 libvtk5-dev

# 下載PCL
git clone https://github.com/PointCloudLibrary/pcl.git
#編譯
cd pcl
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr -DBUILD_apps=ON ..
make -j4
# install
sudo make install

# test
pcl_viewer model.pcd

1.1 基於pcl::visualization::PCLVisualizer 實時顯示點雲流的代碼

include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>



boost::mutex updateModelMutex;

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
        // --------------------------------------------
        // -----Open 3D viewer and add point cloud-----
        // --------------------------------------------
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
        viewer->setCameraPosition(0, 0, 0, 0, 0, 0, 0, 0, -1);
        viewer->setBackgroundColor(0,0,0);
        viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
        viewer->initCameraParameters ();
        return (viewer);
}

void viewerRunner(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer)
{
        while (!viewer->wasStopped ())
        {
               viewer->spinOnce (100);
               boost::this_thread::sleep (boost::posix_time::microseconds (100));
        }
}

void main()
{      
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ> &pcloud1 = *cloud_ptr;
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
        
        pcloud1.points.push_back( pcl::PointXYZ(10, 10, 80) );
        pcloud1.width = cloud_ptr->size();
        pcloud1.height = 1;
        pcloud1.is_dense = true;
        viewer = simpleVis(cloud_ptr);

        boost::thread vthread(&viewerRunner,viewer);
       
        while(1)//循環抓取深度數據
        {
              pcloud1.clear();
              for ( int _row = 0; _row < disp.rows; _row++ )
              {
                   for ( int _col = 0; _col < disp.cols; _col++ )
                   {
                       float x, y, z;
                       pcl::PointXYZ ptemp(x, y, z);
                       pcloud1.points.push_back( ptemp );
                    }
               }
               pcloud1.width = cloud_ptr->size();
               pcloud1.height = 1;
               pcloud1.is_dense = true;
               boost::mutex::scoped_lock updateLock(updateModelMutex);
               viewer->updatePointCloud<pcl::PointXYZ>(cloud_ptr,"sample cloud");
               updateLock.unlock();
               boost::this_thread::sleep (boost::posix_time::microseconds (100));
       }
       vthread.joint();
}
int main()
{
	pcl::visualization::PCLVisualizer viewer(“Cloud”);
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	int frame=0;
	while(1)
	{
		char buf[233];
		if (frame != 0)
		{
			sprintf(buf, “frame%d”, frame);
			viewer.removePointCloud(buf);
		}
		sprintf(buf, “frame%d”, ++frame);
		viewer.addPointCloud(cloud, buf);
		viewer.spinOnce();
	}
	return 0;
}

2. Open3D

  • Open3D是Intel繼OpenCV又一開源鉅作
  • 官方文檔
  • 主要特點
    • 天生支持最愛的Python
    • 採用Google Filament作爲渲染引擎,可在Android, Linux, MacOS和Windows上運行
    • 聚焦可視化和渲染

2.1 編譯環境

  • Ubuntu 18.04+: GCC 5+, Clang 7+
  • macOS 10.14+: XCode 8.0+
  • Windows 10 (64-bit): Visual Studio 2019+

2.2 編譯過程 (Ubuntu 18.04)

  • 下載代碼
git clone --recursive https://github.com/intel-isl/Open3D
  • 安裝依賴
sudo apt install nasm  
util/scripts/install-deps-ubuntu.sh

2.3 設置Python綁定環境 (pybind11)

  • 設置正確的Python執行文件
# Ubuntu 18.04默認安裝了 Python 3.6.9
# 檢查python版本
which python3
python3 -V
# 在CMake中增加
-DPYTHON_EXECUTABLE=/usr/bin/python3
  • 支持Jupyter可視化
# 安裝npm
sudo apt install nodejs
sudo apt install npm
#check npm
node -v
npm -v

2.4 編譯過程

mkdir build; cd build
  • Ubuntu 和 MacOS
cmake -DCMAKE_INSTALL_PREFIX=/usr/local/open3d -DGLIBCXX_USE_CXX11_ABI=OFF -DPYTHON_EXECUTABLE=/usr/bin/python3 ..      
  • Windows
cmake -DCMAKE_INSTALL_PREFIX=/usr/local/open3d \ 
      -DGLIBCXX_USE_CXX11_ABI=OFF \
      -DPYTHON_EXECUTABLE=/usr/bin/python3 \
	  -DBUILD_EIGEN3=ON  \
      -DBUILD_FLANN=ON   \
      -DBUILD_GLEW=ON    \
      -DBUILD_GLFW=ON    \
      -DBUILD_PNG=ON     \
      ..      
  • 執行編譯
make -j4
  • 安裝和卸載C++庫
# 安裝
sudo make install
# 卸載
sudo make uninstall
  • 安裝和卸載Python包
# Create Python package in build/lib/python_package
make python-package
# Create pip wheel in build/lib/python_package/pip_package
make pip-package
# Create conda package in build/lib/python_package/conda_package
make conda-package
# Install pip wheel
sudo make install-pip-package
  • 測試導入Open3D
python -c "import open3d"

2.5 阻塞和非阻塞示例代碼

# -*-coding: utf-8 -*-
import open3d
import numpy as np
import cv2

# 繪製open3d座標系
axis_pcd = open3d.create_mesh_coordinate_frame(size=0.5, origin=[0, 0, 0])
# 在3D座標上繪製點:座標點[x,y,z]對應R,G,B顏色
points = np.array([[0.1, 0.1, 0.1], [1, 0, 0], [0, 1, 0], [0, 0, 1]])
colors = [[1, 1, 1], [1, 0, 0], [0, 1, 0], [0, 0, 1]]
 
test_pcd = open3d.geometry.PointCloud()  # 定義點雲
 
# 方法1(非阻塞顯示)
vis = open3d.Visualizer()
vis.create_window(window_name="Open3D1")
vis.get_render_option().point_size = 3
first_loop = True
# 先把點雲對象添加給Visualizer
vis.add_geometry(axis_pcd)
vis.add_geometry(test_pcd)
while True:
    # 給點雲添加顯示的數據
    points -= 0.001
    test_pcd.points = open3d.utility.Vector3dVector(points)  # 定義點雲座標位置
    test_pcd.colors = open3d.Vector3dVector(colors)  # 定義點雲的顏色
    # update_renderer顯示當前的數據
    vis.update_geometry()
    vis.poll_events()
    vis.update_renderer()
    cv2.waitKey(100)
 
# 方法2(阻塞顯示):調用draw_geometries直接把需要顯示點雲數據
test_pcd.points = open3d.utility.Vector3dVector(points)  # 定義點雲座標位置
test_pcd.colors = open3d.Vector3dVector(colors)  # 定義點雲的顏色
open3d.visualization.draw_geometries([test_pcd] + [axis_pcd], window_name="Open3D2")

2.6 添加自定義的 draw_geometries 函數

import os
import numpy as np
from open3d import *
points = np.random.rand(10000, 3)
point_cloud = PointCloud()
point_cloud.points = Vector3dVector(points)
draw_geometries([point_cloud])

# or
def custom_draw_geometry_with_key_callback(pcd):
    def change_background_to_black(vis):
        opt = vis.get_render_option()
        opt.background_color = np.asarray([0, 0, 0])
        return False
    def load_render_option(vis):
        vis.get_render_option().load_from_json(
                "../../TestData/renderoption.json")
        return False
    def capture_depth(vis):
        depth = vis.capture_depth_float_buffer()
        plt.imshow(np.asarray(depth))
        plt.show()
        return False
    def capture_image(vis):
        image = vis.capture_screen_float_buffer()
        plt.imshow(np.asarray(image))
        plt.show()
        return False
    key_to_callback = {}
    key_to_callback[ord("K")] = change_background_to_black
    key_to_callback[ord("R")] = load_render_option
    key_to_callback[ord(",")] = capture_depth
    key_to_callback[ord(".")] = capture_image
    draw_geometries_with_key_callbacks([pcd], key_to_callback)

custom_draw_geometry_with_key_callback(point_cloud)

2.7 體素下采樣 (Voxel downsampling)

print("Downsample the point cloud with a voxel of 0.05")
downpcd = pcd.voxel_down_sample(voxel_size=0.05)
o3d.visualization.draw_geometries([downpcd], zoom=0.3412,
                                  front=[0.4257, -0.2125, -0.8795],
                                  lookat=[2.6172, 2.0475, 1.532],
                                  up=[-0.0694, -0.9768, 0.2024])

2.8 平面分割 (Plane segmentation)

pcd = o3d.io.read_point_cloud("../../TestData/fragment.pcd")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud], zoom=0.8,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])

在這裏插入圖片描述

2.9 刪除隱藏點

print("Convert mesh to a point cloud and estimate dimensions")
pcd = o3dtut.get_armadillo_mesh().sample_points_poisson_disk(5000)
diameter = np.linalg.norm(np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
o3d.visualization.draw_geometries([pcd])

在這裏插入圖片描述

print("Define parameters used for hidden_point_removal")
camera = [0, 0, diameter]
radius = diameter * 100

print("Get all points that are visible from given view point")
_, pt_map = pcd.hidden_point_removal(camera, radius)

print("Visualize result")
pcd = pcd.select_by_index(pt_map)
o3d.visualization.draw_geometries([pcd])

在這裏插入圖片描述

3. 安裝cmake 3.12.2或更高版本

# 刪除已經安裝的CMake舊版本(非必需項):
apt-get autoremove cmake1
#下載cmake-3.12.2-Linux-x86_64.tar.gz安裝包:
wget https://cmake.org/files/v3.12/cmake-3.12.2-Linux-x86_64.tar.gz
# 解壓請運行以下命令:
tar zxvf cmake-3.12.2-Linux-x86_64.tar.gz
# 創建軟鏈接
#   注: 文件路徑是可以指定的,一般選擇在/opt或者是/usr路徑下,這裏選擇/opt:
mv cmake-3.12.2-Linux-x86_64 /opt/cmake-3.12.2
ln -sf /opt/cmake-3.12.2/bin/*  /usr/bin/
# 然後執行命令檢查一下:
cmake --version

# cmake gui
cmake-gui
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章