SLAM本质剖析-Open3D

Posted 2023-02-05

0. 前言

在深入剖析了​​Ceres​​、​​Eigen​​、​​Sophus​​、​​G2O​​后，以V-SLAM为代表的计算方式基本已经全部讲完。就L-SLAM而言，本系列也讲述了​​PCL​​、与​​GTSAM​​点云计算部分。之前的系列部分作者本以为已经基本讲完，但是近期突然发现还有关于​​Open3D​​的部分还没有写。趁着这次不全来形成一整个系列，方便自己回顾以及后面的人一起学习。

1. Open3D环境安装

这里将Open3D的环境安装分为两个部分：非ROS和ROS环境

非ROS环境

//下载源码
git clone [e-mail]: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 [e-mail]: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";