Open3D Release v0.1.0

v0.1.0

├── CMake
│   └── Findlibusb-1.0.cmake
├── CMakeLists.txt
├── Core
│   ├── Camera
│   ├── CMakeLists.txt
│   ├── Core.h
│   ├── Geometry
│   ├── Integration
│   ├── Odometry
│   ├── Registration
│   └── Utility
├── Experimental
│   ├── CMakeLists.txt
│   ├── EvaluateFeatureMatch
│   ├── EvaluatePCDMatch
│   ├── IntegrateRGBD
│   ├── OdometryRGBD
│   ├── TrimMeshBasedOnPointCloud
│   ├── ViewDistances
│   └── ViewPCDMatch
├── External
│   ├── CMakeLists.txt
│   ├── dirent
│   ├── Eigen
│   ├── flann
│   ├── glew
│   ├── GLFW
│   ├── jsoncpp
│   ├── libjpeg
│   ├── liblzf
│   ├── libpng
│   ├── librealsense
│   ├── pybind11
│   ├── README.txt
│   ├── rply
│   ├── tinyfiledialogs
│   └── zlib
├── IO
│   ├── ClassIO
│   ├── CMakeLists.txt
│   ├── FileFormat
│   └── IO.h
├── LICENSE.txt
├── Python
│   ├── CMakeLists.txt
│   ├── Core
│   ├── IO
│   ├── py3d.cpp
│   ├── py3d_eigen.cpp
│   ├── py3d.h
│   ├── Tutorial
│   └── Visualization
├── Test
│   ├── CMakeLists.txt
│   ├── TestCameraPoseTrajectory.cpp
│   ├── TestData
│   ├── TestDepthCapture.cpp
│   ├── TestFileDialog.cpp
│   ├── TestFileSystem.cpp
│   ├── TestFlann.cpp
│   ├── TestImage.cpp
│   ├── TestLineSet.cpp
│   ├── TestOpenMP.cpp
│   ├── TestPCDFileFormat.cpp
│   ├── TestPointCloud.cpp
│   ├── TestPoseGraph.cpp
│   ├── TestProgramOptions.cpp
│   ├── TestRealSense.cpp
│   ├── TestRegistrationRANSAC.cpp
│   ├── TestTriangleMesh.cpp
│   └── TestVisualizer.cpp
├── Tools
│   ├── CMakeLists.txt
│   ├── ConvertPointCloud.cpp
│   ├── EncodeShader.cpp
│   ├── ManuallyAlignPointCloud
│   ├── ManuallyCropPointCloud.cpp
│   ├── MergeMesh.cpp
│   └── ViewGeometry.cpp
└── Visualization
    ├── CMakeLists.txt
    ├── Shader
    ├── Utility
    ├── Visualization.h
    └── Visualizer

Doc

通过make可以生成此次commit的文档

需要doxygen、sphinx依赖，可以生成html文件，可以直接在浏览器打开

Camera

PinholeCameraIntrinsic：保存相机的内参

PinholeCameraTrajectory：保存相机的位姿（外参）序列, 同时拥有成员PinholeCameraIntrinsic

Geometry

几何类型，维度（Geometry2D, Geometry3D）

Geometry2D

Image
SelectionPolygon

Geometry3D

LineSet : point_set_表示AB两个点集, lines_表示AB两个点集的对应关系
PointCloud: 从rgbd等生成点云
PointCloudPicker: 存储PointCloud中选择的indices
TriangleMesh:
1. triangle normal: 组成三角面的顶点叉积可得
2. vertex normal: 顶点构成的所有三角面的法向量的加和
3. CreateMeshSphere: i循环纬度线,j循环经度线,然后通过alpha,theta角确定坐标
4. CreateMeshCone: 锥体因为底部到顶点为一条线,所以多一个变量split,表示经线的分割程度,默认为1

Normal

ComputeNormal:从一堆先验确定是平面的点里面计算法向量

通过PCA 计算点云法向量

\begin{bmatrix} xx & xy & xz \\ yx & yy & yz \\ zx & zy & zz \end{bmatrix}

Con(x,x)

cov(x, x) = E[(x-E(x))(x-E(x))] \\ =E(x^2)-2*E(x)*E(x)+E(x)^2 = E(x^2) - E(x)^2

Con(x,y)

cov(x, y) = E[(x-E(x))(y-E(y))] \\ =E(x^2)-2*E(x)*E(y)+E(x)*E(y) = E(xy) - E(x)E(y)

EstimateNormals: 通过kd树做到类似于聚类的效果，然后将搜索得到的子集点云丢给ComputeNormal

FPFH feature

Point Feature Histogram: 通过数理统计的方法获得一个用于描述中心点邻域几何信息的直方图

两两点间的局部坐标系：

u=n_1 \\ v=u \times \frac{p_2-p_1}{||p_2-p_1||_2} \\ w=u \times v

特征描述四元组 $(\alpha, \phi, \theta, d)$ ：

\alpha = v*n_2 \\ \phi=u*\frac{p_2-p_1}{d} \\ \theta=arctan(w*n2, u*n2) \\ d = ||p_2 - p1||_2

四元组计算代码实现如下：

Eigen::Vector4d ComputePairFeatures(const Eigen::Vector3d &p1,
		const Eigen::Vector3d &n1, const Eigen::Vector3d &p2,
		const Eigen::Vector3d &n2)
{
    // PFH四元组计算
    // <\theta, \alpha, \phi, d>
	Eigen::Vector4d result;
	Eigen::Vector3d dp2p1 = p2 - p1;
    // d
	result(3) = dp2p1.norm();
	if (result(3) == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	auto n1_copy = n1;
	auto n2_copy = n2;
    // \phi
	double angle1 = n1_copy.dot(dp2p1) / result(3);
	double angle2 = n2_copy.dot(dp2p1) / result(3);
	if (acos(fabs(angle1)) > acos(fabs(angle2))) {
		n1_copy = n2;
		n2_copy = n1;
		dp2p1 *= -1.0;
		result(2) = -angle2;
	} else {
		result(2) = angle1;
	}
	auto v = dp2p1.cross(n1_copy);
	double v_norm = v.norm();
	if (v_norm == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	v /= v_norm;
	auto w = n1_copy.cross(v);
    // \alph
	result(1) = v.dot(n2_copy);
    // \theta
	result(0) = atan2(w.dot(n2_copy), n1_copy.dot(n2_copy));
	return result;
}

Histogram 就是对四元组的特征值进行等分后对每个特征值说在区间频率的统计。

Simplified Point Feature Histogram(SPFH): 将四元组 $(\alpha, \phi, \theta, d)$ 简化为 $(\alpha, \phi, \theta)$

Fast Point Feature Histogram: 基于SPFH将复杂度 $O(nk^2)$ 降低到 $O(nk)$ 。 $n, k$ 分别表示点云具有 $n$ 个点以及中心邻域有 $k$ 个点。

对每个查询 $p$ , 只计算其与相邻邻居点间的关联。
使用 $k$ 个邻居点的邻居的 SPFH 值对最终的点 $p$ 的 FPFH 值进行加权。

FPFH(p)=SPF(p)+\frac{1}{k} \sum_{i=1}^{k} \frac{1}{\omega_k}*SPF(p_k)

$\omega_k$ 表示查询点 $p$ 与邻居点 $p_k$ 间在某个度量空间下的距离。

按照原论文中也就是上一条公式，实现应该如下所示

fast_histArray = np.zeros_like(histArray)
for i in range(N):
	k = len(indNeigh[i])
	for j in range(k):
		spfh_sum = histArray[indNeigh[i][j]]*(1/distList[i][j])
	
	fast_histArray[i, :] = histArray[i, :] + (1/k)*spfh_sum

但是 Open3D 的实现的加权方式应该是有所不同的：

for (int i = 0; i < (int)input.points_.size(); i++) {
	const auto &point = input.points_[i];
	std::vector<int> indices;
	std::vector<double> distance2;
	if (kdtree.Search(point, search_param, indices, distance2) > 1) {
		double sum[3] = {0.0, 0.0, 0.0};
		for (size_t k = 1; k < indices.size(); k++) {
			// skip the point itself
			// \omega_k
			double dist = distance2[k];
			if (dist == 0.0)
				continue;
			for (int j = 0; j < 33; j++) {
				//SPF(p_k)
				double val = spfh->data_(j, indices[k]) / dist;
				sum[j / 11] += val;
				// \sum_{i=1}^{k} \frac{1}{\omega_k}*SPF(p_k)
				feature->data_(j, i) += val;
			}
		}
		for (int j = 0; j < 3; j++)
			if (sum[j] != 0.0) sum[j] = 100.0 / sum[j];
		for (int j = 0; j < 33; j++) {
			feature->data_(j, i) *= sum[j / 11];
			// The commented line is the fpfh function in the paper.
			// But according to PCL implementation, it is skipped.
			// Our initial test shows that the full fpfh function in the
			// paper seems to be better than PCL implementation. Further
			// test required.

			// SPF(p): spfh->data_(j, i)
			feature->data_(j, i) += spfh->data_(j, i);
		}
	}
}

Registration

RegistrationICP

输入为

max_correspondence_distance：kd树的搜索半径，这个变量的存在意义是在无先验匹配点对的场景下需要代码自己计算匹配点对。于是 Open3D 使用的是假设匹配点对的空间相隔较近。所以在无先验匹配的情况下，一个良好的初始值非常重要。
init：两个点云间的初始位姿变换，默认为单位矩阵。
ICPConvergenceCriteria：用来对 ICP 迭代停止的时间点进行判断。
1. relative_fitness
2. relative_rmse_
3. max_iteration_

if (std::abs(backup.fitness_ - result.fitness_) <
		criteria.relative_fitness_ && std::abs(backup.inlier_rmse_ -
		result.inlier_rmse_) < criteria.relative_rmse_) {
	break;
}

GetRegistrationResultAndCorrespondences：最主要的 ICP 调用方法。内部使用 OpenMP 的多线程方法，对每个点的最邻近点的距离进行计算，然后将他们的均方根作为 rmse。返回值为：

符合要求的匹配点对。
inlier_rmse_：匹配点对的均方根。
fitness_：匹配点对占点云点总数的比例。

RegistrationRANSACBasedOnCorrespondence

corres: std::vector<Eigen::Vector2i>，代表 index1 和 index2 的数组。
ransac_n：做 ransac 算法使用的随机选取的数目，默认为 6.4。

1
2
3

for (int j = 0; j < ransac_n; j++) {
	ransac_corres[j] = corres[std::rand() % (int)corres.size()];
}

TransformationEstimationPointToPoint::ComputeTransformation：
根据对应点计算 Transformation 矩阵。

底层调用的是 Eigen::umeyama 对初始点对进行计算。参考论文为 “Least-squares estimation of transformation parameters between two point patterns”, Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573

后续就是通常的 RANSAC 的最小误差迭代过程。

CorrespondenceChecker

CorrespondenceChecker 作为基类，具体逻辑由子类实现。所有逻辑输入均有变量 Correspondence

CorrespondenceCheckerBasedOnEdgeLength：使用 similarity_threshold_ 判断多边形是否具有相同的边长。如果两组点满足对应关系，那么任选一组对应点，与其剩下的所有点间的距离应该都相似。复杂度 $O(n^2)$
CorrespondenceCheckerBasedOnDistance：如果两组点满足对应关系，那个给定一个 transformation 变换，那个两组点应该近似重合。（无法保证点间的对应关系）复杂度 $O(n)$
CorrespondenceCheckerBasedOnNormal：如果两组点满足对应关系，那个给定一个 transformation 变换，那个两组点应该满足对应点法向量方向近似一致。复杂度 $O(n)$

RegistrationRANSACBasedOnFeatureMatching

首先外部会进行 FPFH 特征计算，并且将一组特征传入这个对齐算法。

首先在 FPFH 的特征向量上建立 kdtree。
然后是一个 ransac 循环，随机选择 n 个特征点。
每个循环中根据用户设定的最近邻数量选择一个目标，复数个就随机选择一个。
使用上一节的 CorrespondenceChecker 方法对两组匹配的特征进行检查。
最后再使用 RegistrationRANSACBasedOnCorrespondence 进行 transformation 计算。