作者丨lovely_yoshino
0. 前言
在深入剖析了Ceres、Eigen、Sophus、G2O后，以V-SLAM为代表的计算方式基本已经全部讲完。就L-SLAM而言，本系列也讲述了PCL、与GTSAM点云计算部分。
之前的系列部分作者本以为已经基本讲完，但是近期突然发现还有关于Open3D的部分还没有写。趁着这次不全来形成一整个系列，方便自己回顾以及后面的人一起学习。
1. Open3D环境安装
这里将Open3D的环境安装分为两个部分：非ROS和ROS环境
非ROS环境
//下载源码
git clone git@github.com:intel-isl/Open3D.git
git submodule update --init --recursive
//安装依赖
cd open3d
util/scripts/install-deps-ubuntu.sh
//编译安装
mkdir build
cd build
sudo cmake -DCMAKE_INSTALL_PREFIX=/opt/Open3D/ -DBUILD_EIGEN3=ON -DBUILD_GLEW=ON -DBUILD_GLFW=ON -DBUILD_JSONCPP=ON -DBUILD_PNG=ON -DPYTHON_EXECUTABLE=/usr/bin/python ..
sudo make -j8
sudo make install
ROS环境
//更新cmake
sudo apt-add-repository 'deb https://apt.kitware.com/ubuntu/ bionic main'
sudo apt-get update
sudo apt-get install cmake
//安装Open3D
git clone --recursive https://github.com/intel-isl/Open3D
cd Open3D && source util/scripts/install-deps-ubuntu.sh
mkdir build && cd build
cmake -DBUILD_EIGEN3=ON -DBUILD_GLEW=ON -DBUILD_GLFW=ON -DBUILD_JSONCPP=ON -DBUILD_PNG=ON -DGLIBCXX_USE_CXX11_ABI=ON -DPYTHON_EXECUTABLE=/usr/bin/python -DBUILD_UNIT_TESTS=ON ..
make -j4
sudo make install
//基于Open3D的ros程序
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone git@github.com:unr-arl/open3d_ros.git
cd ..
catkin config -DCMAKE_BUILD_TYPE=Release
catkin build open3d_ros
2. Open3D示例
Open3D的操作和PCL类似，都是利用源码读取ply点云后, 并做ransec平面分割的操作。
//Open3D
#include "Open3D/Open3D.h"
//Eigen
#include "Eigen/Dense"
/* RANSAC平面分割 */
void testOpen3D::pcPlaneRANSAC(const QString &pcPath)
{
int width = 700, height = 500;
auto cloud_ptr = std::make_shared<open3d::geometry::PointCloud>();
if (!open3d::io::ReadPointCloud(pcPath.toStdString(), *cloud_ptr)) { return; }
open3d::visualization::DrawGeometries({ cloud_ptr }, "Point Cloud 1", width, height);
/***** 距离阈值，平面最小点数，最大迭代次数。返回平面参数和内点 *****/
double tDis = 0.05;
int minNum = 3;
int numIter = 100;
std::tuple<Eigen::Vector4d, std::vector<size_t>> vRes = cloud_ptr->SegmentPlane(tDis, minNum, numIter);
//ABCD
Eigen::Vector4d para = std::get<0>(vRes);
//内点索引
std::vector<size_t> selectedIndex = std::get<1>(vRes);
//内点赋红色
std::shared_ptr<open3d::geometry::PointCloud> inPC = cloud_ptr->SelectByIndex(selectedIndex, false);
const Eigen::Vector3d colorIn = { 255,0,0 };
inPC->PaintUniformColor(colorIn);
//外点赋黑色
std::shared_ptr<open3d::geometry::PointCloud> outPC = cloud_ptr->SelectByIndex(selectedIndex, true);
const Eigen::Vector3d colorOut = { 0,0,0 };
outPC->PaintUniformColor(colorOut);
//显示
open3d::visualization::DrawGeometries({ inPC,outPC }, "RANSAC平面分割", width, height);
}
对于Open3D而言，PCL可以做到的，其自身也可以做到，下面部分代码为Open3D的ICP匹配
#include <opencv2/opencv.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include "Open3D/Registration/GlobalOptimization.h"
#include "Open3D/Registration/PoseGraph.h"
#include "Open3D/Registration/ColoredICP.h"
#include "Open3D/Open3D.h"
#include "Open3D/Registration/FastGlobalRegistration.h"
using namespace open3d;
using namespace std;
using namespace registration;
using namespace geometry;
using namespace cv;
void main()
{
open3d::geometry::PointCloud source, target;
open3d::io::ReadPointCloud("C:/Users/chili1080/Desktop/Augmented ICL-NUIM Dataset-jyx/livingroom1-fragments-ply/cloud_bin_0.ply", source);
open3d::io::ReadPointCloud("C:/Users/chili1080/Desktop/Augmented ICL-NUIM Dataset-jyx/livingroom1-fragments-ply/cloud_bin_1.ply", target);
Eigen::Vector3d color_source(1, 0.706, 0);
Eigen::Vector3d color_target(0, 0.651, 0.929);
source.PaintUniformColor(color_source);
target.PaintUniformColor(color_target);
double th = 0.02;
open3d::registration::RegistrationResult res = open3d::registration::RegistrationICP(source, target, th, Eigen::Matrix4d::Identity(),
TransformationEstimationPointToPoint(false), ICPConvergenceCriteria(1e-6, 1e-6, 300));
//显示配准结果 fitness 算法对这次配准的打分
//inlier_rmse 表示的是 root of covariance, 也就是所有匹配点之间的距离的总和除以所有点的数量的平方根
//correspondence_size 代表配准后吻合的点云的数量
cout << "fitness: "<<res.fitness_<<" inlier rmse:"<<res.inlier_rmse_<<" correspondence_set size:"<<res.correspondence_set_.size()<<endl;
cout << "done here";
}
3. Open3D在SLAM当中的应用
由于open3D相较于pcl而言对Python的兼容性会更好，所以在使用python做slam时候，大多数会使用open3d。这里也不例外。提供了一份基于Open3D的点云映射匹配。
import open3d as o3d
import numpy as np
import copy
def visualize(pcd):
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
opt = vis.get_render_option()
opt.background_color = np.asarray([0, 0, 0])
opt.light_on = False
opt.point_size = 2
vis.run()
vis.destroy_window()
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
# source_temp.paint_uniform_color([1, 0.706, 0])
# target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target_temp])
def feature_extraction(pcd, voxel_size, gamma=False):
if gamma:
color = np.asarray(pcd.colors)
color = np.minimum(np.power(color, 0.6), 1)
pcd.colors = o3d.utility.Vector3dVector(color)
# pcd_down, _ = pcd_down.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
print(":: Estimate normal with search radius %.3f." % voxel_size)
pcd.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size, max_nn=30))
print(":: Downsample with a voxel size %.3f." % voxel_size)
pcd_down = pcd.voxel_down_sample(voxel_size)
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_fpfh = o3d.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
return pcd_down, pcd_fpfh
def prepare_dataset(voxel_size):
print(":: Load two point clouds and disturb initial pose.")
source = o3d.io.read_point_cloud('/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia1.pcd')
# target = o3d.io.read_point_cloud('/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia2.pcd')
target = o3d.io.read_point_cloud('/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia3.pcd')
# trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
# [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
# source.transform(trans_init)
draw_registration_result(source, target, np.identity(4))
source_down, source_fpfh = feature_extraction(source, voxel_size)
target_down, target_fpfh = feature_extraction(target, voxel_size)
return source, target, source_down, target_down, source_fpfh, target_fpfh
def execute_global_registration(source_down, target_down, source_fpfh,
target_fpfh, voxel_size):
distance_threshold = voxel_size * 1.5
print(":: RANSAC registration on downsampled point clouds.")
print(" Since the downsampling voxel size is %.3f," % voxel_size)
print(" we use a liberal distance threshold %.3f." % distance_threshold)
result = o3d.registration.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
o3d.registration.TransformationEstimationPointToPoint(False), 4, [
o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.registration.CorrespondenceCheckerBasedOnDistance(
distance_threshold)
], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
return result
def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size, initial_guess):
distance_threshold = voxel_size * 0.4
print(":: Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. This time we use a strict")
print(" distance threshold %.3f." % distance_threshold)
result = o3d.registration.registration_icp(
source, target, distance_threshold, initial_guess,
o3d.registration.TransformationEstimationPointToPlane())
return result
def mapping(pcd_list, voxel_size):
for frame_idx in range(len(pcd_list) - 1):
# 读取数据
if frame_idx == 0:
map_pcd = pcd_list[frame_idx]
map_down, map_fpfh = feature_extraction(map_pcd, voxel_size=voxel_size, gamma=True)
source_pcd = pcd_list[frame_idx + 1]
source_down, source_fpfh = feature_extraction(source_pcd, voxel_size=voxel_size, gamma=True)
# 全局ICP
result_ransac = execute_global_registration(source_down, map_down,
source_fpfh, map_fpfh,
voxel_size)
print(result_ransac)
# refine结果
result_icp = refine_registration(source_pcd, map_pcd, source_fpfh, map_fpfh,
voxel_size, initial_guess=result_ransac.transformation)
print(result_icp)
# draw_registration_result(source_pcd, map_pcd,
# result_icp.transformation)
# 更新map
map_pcd += source_pcd.transform(result_icp.transformation)
map_down, map_fpfh = feature_extraction(map_pcd, voxel_size=voxel_size)
return map_pcd
if __name__ == "__main__":
voxel_size = 0.3 # means 5cm for the dataset
# 点云数据路径
# 每帧之间需要有相互
data_list = [
'/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia1.pcd',
'/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia2.pcd',
'/home/hermit/Code/ACSC/experiments/RGBMAP_v2/RGBMap_avia3.pcd',
]
# 读取点云数据集
pcds = [o3d.io.read_point_cloud(path) for path in data_list]
map_pcd = mapping(pcd_list=pcds, voxel_size=voxel_size)
# 绘制地图
visualize(map_pcd)
o3d.io.write_point_cloud("mapping.pcd", map_pcd)
参考链接
- https://bleepcoder.com/cn/open3d/335891727/examples-for-how-to-use-open3d-with-pcl-library-and-ros
- 本文仅做学术分享，如有侵权，请联系删文。
