SfM兩視圖三維點雲重建--【VS2015+OpenCV3.4+PCL1.8】

概述

視覺三維重建，是指使用相機採集的圖片、根據相關知識推導目標物體三維信息的過程。這裏借鑑《視覺SLAM14講》中的分類方法，將視覺三維重建分爲基於特徵點和非提取特徵點的重建。本文旨在使用基於特徵點的重建方法完成相鄰兩張圖片間的三維重建。

步驟

基於特徵點的視覺三維重建方法主要包括如下流程：

提取單張圖片特徵點；
多張圖片間特徵點的匹配；
利用對極幾何約束，使用SfM恢復兩相機間的三維變換關係；
使用三角量測法（或稱三角化法），重構二維圖像對應點的三維點信息。

按照我的理解，上述流程可以概括爲如下兩部分：求解相機內外參、三維重建。

求解相機內外參：
提取特徵點並進行匹配只是爲了組成匹配對，以方便計算基本矩陣F/本質矩陣E（對極幾何約束），而計算出矩陣E或F後，即可進行SVD分解得到兩相機間的三維變換關係（旋轉矩陣R、平移矩陣t）。至此，是爲求解相機內外參的過程。當然，也可以使用相機標定的方式獲取相機內外參數，但多張重建會比較繁瑣。
三維重建
在已知兩圖片間變換矩陣的情況下，就可以使用三角化進行三維重建了。重建後的三維點是以第一張圖片對應的相機座標系爲世界座標系。

這裏只是進行簡單的介紹，並不涉及複雜的原理，有興趣的可參考14講中的內容，網上也有許多優秀博文可供參考。

代碼

環境：Win10+VS2015+OpenCV3.4+PCL1.8

#include <iostream>
#include <opencv2/opencv.hpp>

#include <opencv2/features2d/features2d.hpp> 
#include <opencv2/xfeatures2d/nonfree.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace cv;
using namespace pcl;
using namespace cv::xfeatures2d;

// ratio & symmetry test
void ratioTest(vector<vector<DMatch>> &matches, vector<DMatch> &goodMatches);
void symmetryTest(const vector<DMatch>& matches1, const vector<DMatch>& matches2, vector<DMatch>& symMatches);

// 從匹配對中提取特徵點
void fillPoints(vector<DMatch> goodMatches, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<Point2f>& points1, vector<Point2f>& points2);

// 三維重建
void reconstruct(Mat& K, Mat& fundamentalMatrix, vector<Point2f>& points1, vector<Point2f>& points2, vector<Point3f>& points3D, Mat_<double>& R, Mat_<double>& t);

// 獲取關鍵點RGB
void getPointColor(vector<Point2f> points1, Mat BaseImageLeft, vector<Vec3b>& colors);

int main(int argc, char* argv[])
{
	// PCL可視化
	PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
	boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D viewer")); // 實例化PCLVisualizer對象，窗口命名爲3D viewer

	Mat K;			// 內參數矩陣
	Matx34d P, P1;  // 兩圖片的相機座標

	// 1.讀入圖片
	Mat BaseImageLeft = imread(".\\images\\004.png", -1);
	Mat BaseImageRight = imread(".\\images\\006.png", -1);
	if (BaseImageLeft.empty() || BaseImageRight.empty())
	{
		cout << "ERROR! NO IMAGE LOADED!" << endl;
		return -1;
	}
	cout << "processing..." << endl;
	// 2.SIFT提取特徵點
	Ptr<Feature2D> detector = xfeatures2d::SIFT::create(0, 3, 0.04, 10);

	vector<KeyPoint> keypoints_1, keypoints_2; // 關鍵點
	Mat descriptors_1, descriptors_2;		   // 描述符

	detector->detectAndCompute(BaseImageLeft, noArray(), keypoints_1, descriptors_1);
	detector->detectAndCompute(BaseImageRight, noArray(), keypoints_2, descriptors_2);

	// 3.Flann匹配特徵點
	vector<vector<DMatch>> matches1, matches2;
	vector<DMatch> goodMatches1, goodMatches2, goodMatches, outMatches;

	FlannBasedMatcher matcher;
	// 1個descriptors_1[i]對應1個matches[i]，1個matches[i]對應2個DMatch(2 nearest)
	matcher.knnMatch(descriptors_1, descriptors_2, matches1, 2);// find 2 nearest neighbours, match.size() = query.rowsize()
	matcher.knnMatch(descriptors_2, descriptors_1, matches2, 2);

	// 4.使用Ratio Test和Symmetry Test消除誤匹配點，提高重建精度
	ratioTest(matches1, goodMatches1);
	ratioTest(matches2, goodMatches2);

	symmetryTest(goodMatches1, goodMatches2, goodMatches);// Symmetry Test

	// 5.計算基本矩陣F
	Mat fundamentalMatx;
	vector<Point2f> points1, points2;

	if (goodMatches.size() < 30)
	{
		cerr << "ERROR: NOT ENOUGH MATCHES" << endl;
	}
	else
	{
		// 使用RANSAC消除誤匹配（保留inliers值爲1的特徵點）
		fillPoints(goodMatches, keypoints_1, keypoints_2, points1, points2);
		vector<uchar> inliers(points1.size(), 0); // 內點爲1，外點爲0
		fundamentalMatx = findFundamentalMat(points1, points2, inliers, CV_FM_RANSAC, 3.0, 0.99);

		vector<DMatch>::const_iterator itM = goodMatches.begin();
		for (vector<uchar>::const_iterator itIn = inliers.begin(); itIn != inliers.end(); ++itIn, ++itM)
		{
			if (*itIn) // 若爲內點
				outMatches.push_back(*itM); // 用於計算F的最終匹配對
		}
		points1.clear();
		points2.clear();

		// 計算基本矩陣F
		fillPoints(outMatches, keypoints_1, keypoints_2, points1, points2);
		fundamentalMatx = findFundamentalMat(points1, points2, CV_FM_8POINT);
	}

	// 6.三角化三維重建
	K = (Mat_<double>(3, 3) << 2759.48, 0, 1520.69, 0, 2764.16, 1006.81, 0, 0, 1); // Fountain的內參數矩陣

	vector<Point3f> points3D;
	Mat_<double> R, t; // 用於顯示座標軸的位置
	reconstruct(K, fundamentalMatx, points1, points2, points3D, R, t);

	cout << "\t3D Reconstruction was Finished." << endl;

	// 7.可視化
	vector<Vec3b> points_Colors;
	getPointColor(points1, BaseImageLeft, points_Colors);

	for (size_t i = 0; i < points3D.size(); i++)
	{
		PointXYZRGB p;
		p.x = points3D[i].x;
		p.y = points3D[i].y;
		p.z = points3D[i].z;

		p.b = points_Colors[i][0];
		p.g = points_Colors[i][1];
		p.r = points_Colors[i][2];

		cloud->points.push_back(p);
	}
	// 在每個相機處顯示座標軸
	// cam1（世界座標系）
	Eigen::Matrix4f transformationMatrix;
	transformationMatrix <<
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1;

	Eigen::Affine3f postion1;
	postion1.matrix() = transformationMatrix.inverse();

	// cam2
	transformationMatrix <<
		R(0, 0), R(0, 1), R(0, 2), t(0),
		R(1, 0), R(1, 1), R(1, 2), t(1),
		R(2, 0), R(2, 1), R(2, 2), t(2),
		0, 0, 0, 1;
	Eigen::Affine3f postion2;
	postion2.matrix() = transformationMatrix.inverse();

	viewer->addCoordinateSystem(0.4, postion1);
	viewer->addCoordinateSystem(0.4, postion2);

	viewer->setBackgroundColor(0, 0, 0); // 設置背景顏色
	viewer->addPointCloud<PointXYZRGB>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_OPACITY, 2, "sample cloud");

	while (!viewer->wasStopped()) {
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}

	waitKey();
	return 0;
}
// 比率檢測
void ratioTest(vector<vector<DMatch>> &matches, vector<DMatch> &goodMatches)
{
	for (vector<vector<DMatch>>::iterator matchIterator = matches.begin(); matchIterator != matches.end(); ++matchIterator)
		if (matchIterator->size() > 1)
			if ((*matchIterator)[0].distance < (*matchIterator)[1].distance *0.8)
				goodMatches.push_back((*matchIterator)[0]);
}
// 對稱性檢測
void symmetryTest(const vector<DMatch>& matches1, const vector<DMatch>& matches2, vector<DMatch>& symMatches)
{
	symMatches.clear();
	for (vector<DMatch>::const_iterator matchIterator1 = matches1.begin(); matchIterator1 != matches1.end(); ++matchIterator1)
		for (vector<DMatch>::const_iterator matchIterator2 = matches2.begin(); matchIterator2 != matches2.end(); ++matchIterator2)
			if ((*matchIterator1).queryIdx == (*matchIterator2).trainIdx && (*matchIterator1).trainIdx == (*matchIterator2).queryIdx)
				symMatches.push_back(*matchIterator1);
}
// 由匹配對提取特徵點對
void fillPoints(vector<DMatch> goodMatches, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<Point2f>& points1, vector<Point2f>& points2)
{
	for (vector<DMatch>::const_iterator it = goodMatches.begin(); it != goodMatches.end(); ++it)
	{
		points1.push_back(keypoints1[it->queryIdx].pt); // Get the position of left keypoints
		points2.push_back(keypoints2[it->trainIdx].pt); // Get the position of right keypoints
	}
}

void reconstruct(Mat& K, Mat& fundamentalMatrix, vector<Point2f>& points1, vector<Point2f>& points2, vector<Point3f>& points3D, Mat_<double>& R, Mat_<double>& t)
{

	Mat_<double> E = K.t() * fundamentalMatrix * K; // E = (K').transpose() * F * K

	// 使用recoverPose解算R、T

	// 根據內參數矩陣獲取相機的焦距和光心座標（主點座標）
	double focal_length = 0.5 * (K.at<double>(0) + K.at<double>(4));
	Point2d principle_point(K.at<double>(2), K.at<double>(5));

	int pass_count = recoverPose(E, points1, points2, R, t, focal_length, principle_point, noArray());

	// P、P1爲外參數矩陣，即相機座標系到世界座標系的轉換關係
	Mat P = (Mat_<double>(3, 4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); // 視爲世界座標系，投影矩陣爲標準型矩陣：[I 0]
	Mat P1 = (Mat_<double>(3, 4) << R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), R(2, 0), R(2, 1), R(2, 2), t(2));

	// 歸一化，將像素座標轉換到相機座標
	/*
	方式1：歸一化變換矩陣，triangulatePoints()中P爲：內參*外參矩陣，points爲像素座標：
	P = K * P
	方式2：歸一化座標，triangulatePoints()中P爲外參數矩陣，points爲相機座標
	X = K.inv() * x;
	*/
	P = K*P;
	P1 = K*P1;

	Mat points4D; // 齊次座標形式：4*N：[x y z w].t()
	triangulatePoints(P, P1, points1, points2, points4D);

	// 轉成非齊次座標
	for (size_t i = 0; i < points4D.cols; i++)
	{
		Mat_<float> c = points4D.col(i);
		c /= c(3);

		Point3f p(c(0), c(1), c(2));
		points3D.push_back(p);
	}
}
// 獲取關鍵點的RGB值
void getPointColor(vector<Point2f> points1, Mat BaseImageLeft, vector<Vec3b>& colors)
{
	colors.resize(points1.size());
	for (size_t i = 0; i < points1.size(); i++)
	{
		Point2f& p = points1[i];
		if (p.x <= BaseImageLeft.cols && p.y <= BaseImageLeft.rows)
			colors[i] = BaseImageLeft.at<Vec3b>(p.y, p.x);
	}
}

結果

使用的原圖：

重建結果：

注意事項

使用任意兩張相鄰圖片進行重建時，內參數K略有不同，但不影響重建效果。但要儘量選擇相鄰的圖片，否則可能會因爲匹配的特徵點較少而重建失敗。
本文代碼在分解本質矩陣E時，使用的是recoverPose()，當然也可以使用SVD分解本質矩陣。但是使用SVD分解會得到4個可能的解，只有選擇正確的R、T才能完成重建,否則歸一化的座標之後無法正常顯示；而使用recoverPose則能直接得到正確的R、t。

// 使用SVD分解本質矩陣E，
// 但要確認使用4個解中的哪個正確：把任意一點代入四種解中，檢測該點在兩個相機下的深度，當兩相機下深度值均爲正時的解，即爲正確的解--《視覺SLAM14講P148》
SVD svd(E, SVD::MODIFY_A);

Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1); // equation $9.13 page 258（MVG）
Matx33d Wt(0, 1, 0, -1, 0, 0, 0, 0, 1); // W的轉置矩陣

// SVD分解的4個解
Mat_<double> R1 = svd.u * Mat(W) * svd.vt; // equation $9.14 page 258，SVD分解：R=UW(Vt)或R=U(Wt)(Vt)
Mat_<double> R2 = svd.u * Mat(Wt) * svd.vt;

Mat_<double> t1 = svd.u.col(2); // t = U(0, 0, 1).transpose() = u3 page 259
Mat_<double> t2 = -svd.u.col(2);

在使用triangulatePoints()進行三維重建時，有兩種歸一化方式，即歸一化變換矩陣和歸一化點座標，具體操作方法見代碼。這兩種方式本質是相同的，只不過是歸一化的先後順序而已。

參考

OpenCV實現SfM（二）：雙目三維重建
對極幾何-本質矩陣-基本矩陣 --推導本質矩陣時，在左叉乘T之後，左點乘的應該是P’的轉置，而不是P’。
完整工程下載：SfM稀疏三維點雲重建【VS2015+OpenCV3.4+PLC1.8】–完整工程文件

SfM兩視圖三維點雲重建--【VS2015+OpenCV3.4+PCL1.8】

概述

步驟

代碼

結果

注意事項

參考

釘釘打卡速度慢

Nginx R31 doc 官方文檔-01-nginx 如何安裝

Qt/C++音視頻開發74-合併標籤圖形/生成yolo運算結果圖形/文字和圖形合併成一個/水印濾鏡

挑戰程序設計競賽 2.2章習題 POJ - 3617 Best Cow Line 貪心

字節面試：MySQL什麼時候鎖表？如何防止鎖表？

.NET8連接SQL SERVER 2008 R2 報：證書鏈是由不受信任的頒發機構頒發的

golang開發環境搭建(win10)

python計算機視覺學習筆記——PIL庫的用法

Golang初學：獲取程序內存使用情況，std runtime

VS中【F5】與【Ctrl+F5】及【F7】與【Ctrl+F7】的區別

SfM兩視圖三維點雲重建--【VS2015+OpenCV3.4+PCL1.8】

PCL可視化點雲顏色特徵

新松機器人：【示教編程】+【PC離線編程】+【PLC遠程IO】相結合

Bumblebee相機：標定、分離圖像+保存成cv::Mat格式

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結