稠密重建"/>
视觉SLAM之单目稠密重建
参考高翔的slam十四讲，单目相机在已知轨迹下的稠密深度估计。
数据集参考：
将使用 REMODE[113, 109] 的测试数据集。它提供了一架无人机采集
的单目俯视图像，共有 200 张，同时提供了每张图像的真实位姿。
.zip 处下载。
下载慢的可以去本人上传的资源处下载。
解压后，将在 test_-data/Images 中有从 0 至 200 的所有图像，并在 test_data 目录下可以看到一个文本文件，它记录了每张图像对应的位姿。
解释都写在内码中：
#include <iostream>
#include <vector>
#include <fstream>using namespace std;#include <boost/timer.hpp>// for sophus
#include <sophus/se3.hpp>using Sophus::SE3d;// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace Eigen;#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>using namespace cv;/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。
***********************************************/// ------------------------------------------------------------------
// parameters
const int boarder = 20; // 边缘宽度?
//图像640×480
const int width = 640; // 图像宽度
const int height = 480; // 图像高度
// 相机内参
const double fx = 481.2f;
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3; // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1; // 收敛判定：最小方差
const double max_cov = 10; // 发散判定：最大方差// ------------------------------------------------------------------函数声明
// 重要的函数
/// 从 REMODE 数据集读取数据
bool readDatasetFiles(const string &path,//路径vector<string> &color_image_files,//彩图vector<SE3d> &poses,//位姿cv::Mat &ref_depth
);
/*** 根据新的图像更新深度估计* @param ref 参考图像* @param curr 当前图像* @param T_C_R 参考图像到当前图像的位姿* @param depth 深度* @param depth_cov 深度方差* @return 是否成功*/
bool update(const Mat &ref,const Mat &curr,const SE3d &T_C_R,Mat &depth,Mat &depth_cov2
);/*** 极线搜索* @param ref 参考图像* @param curr 当前图像* @param T_C_R 位姿* @param pt_ref 参考图像中点的位置* @param depth_mu 深度均值* @param depth_cov 深度方差* @param pt_curr 当前点* @param epipolar_direction 极线方向* @return 是否成功*//**************************极线搜索**********************************************/
bool epipolarSearch(const Mat &ref,const Mat &curr,const SE3d &T_C_R,const Vector2d &pt_ref,const double &depth_mu,const double &depth_cov,Vector2d &pt_curr,Vector2d &epipolar_direction
);/*** 更新深度滤波器* @param pt_ref 参考图像点* @param pt_curr 当前图像点* @param T_C_R 位姿* @param epipolar_direction 极线方向* @param depth 深度均值* @param depth_cov2 深度方向* @return 是否成功*/
bool updateDepthFilter(const Vector2d &pt_ref,const Vector2d &pt_curr,const SE3d &T_C_R,const Vector2d &epipolar_direction,Mat &depth,Mat &depth_cov2
);/*** 计算 NCC 评分* @param ref 参考图像* @param curr 当前图像* @param pt_ref 参考点* @param pt_curr 当前点* @return NCC评分*/
double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr);
/**********************************************************************************************/
// 双线性灰度插值
inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt)
{uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))];double xx = pt(0, 0) - floor(pt(0, 0));double yy = pt(1, 0) - floor(pt(1, 0));return ((1 - xx) * (1 - yy) * double(d[0]) +xx * (1 - yy) * double(d[1]) +(1 - xx) * yy * double(d[img.step]) +xx * yy * double(d[img.step + 1])) / 255.0;
}
/*********************************************************************************************/
// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);
/*******************像素到相机坐标系的转换************************************/
// 像素到相机坐标系
inline Vector3d px2cam(const Vector2d px) {return Vector3d((px(0, 0) - cx) / fx,(px(1, 0) - cy) / fy,1);
}// 相机坐标系到像素
inline Vector2d cam2px(const Vector3d p_cam) {return Vector2d(p_cam(0, 0) * fx / p_cam(2, 0) + cx,p_cam(1, 0) * fy / p_cam(2, 0) + cy);
}
/*******************像素到相机坐标系的转换************************************/
// 检测一个点是否在图像边框内
inline bool inside(const Vector2d &pt) {return pt(0, 0) >= boarder && pt(1, 0) >= boarder&& pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}// 显示极线匹配
void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr);
//ref:参考图像;curr:当前图像；px_ref:参考点;px_curr:当前点
// 显示极线
void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,const Vector2d &px_max_curr);/// 评测深度估计
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------/*****************************main*******************************************************/
int main(int argc, char **argv)
{if (argc != 2) {cout << "Usage: dense_mapping path_to_test_dataset" << endl;return -1;}Eigen::Vector3d I;I<<1,2,3;Eigen::Vector3d m;m<<1,2,3;cout<<"@@@@@@"<<I.dot(m)<<endl;// 从数据集读取数据vector<string> color_image_files;vector<SE3d> poses_TWC;Mat ref_depth;bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);if (ret == false) {cout << "Reading image files failed!" << endl;return -1;}cout << "read total " << color_image_files.size() << " files." << endl;// 第一张图作为参考图像Mat ref = imread(color_image_files[0], 0); // gray-scale imageSE3d pose_ref_TWC = poses_TWC[0];//第一张图像的位姿double init_depth = 3.0; // 深度初始值double init_cov2 = 3.0; // 方差初始值Mat depth(height, width, CV_64F, init_depth); // 深度图Mat depth_cov2(height, width, CV_64F, init_cov2); // 深度图方差//遍历每一张图像for (int index = 1; index < color_image_files.size(); index++){cout << "*** loop " << index << " ***" << endl;Mat curr = imread(color_image_files[index], 0);if (curr.data == nullptr) continue;SE3d pose_curr_TWC = poses_TWC[index];SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系： T_C_W * T_W_R = T_C_R 参考图像到当前图像的位姿update(ref, curr, pose_T_C_R, depth, depth_cov2);//更新深度估计evaludateDepth(ref_depth, depth);//评估深度估计plotDepth(ref_depth, depth);imshow("image", curr);waitKey(1);}cout << "estimation returns, saving depth map ..." << endl;imwrite("depth.png", depth);//保存深度图cout << "done." << endl;return 0;
}
//读取数据集中的文件
bool readDatasetFiles(const string &path,vector<string> &color_image_files,std::vector<SE3d> &poses,cv::Mat &ref_depth){ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");if (!fin){cout<<"^^^^^^^^^^^^^^^^^^"<<endl;return false;}while (!fin.eof()) {// 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCWstring image;fin >> image;double data[7];for (double &d:data) fin >> d;cout<<path + string("/images/") + image<<endl;color_image_files.push_back(path + string("/images/") + image);poses.push_back(SE3d(Quaterniond(data[6], data[3], data[4], data[5]),Vector3d(data[0], data[1], data[2])));//cout<<"poses,data()"<<poses.()<<endl;if (!fin.good()) break;}fin.close();//读取文件完成后关闭.// load reference depth装载每个像素点的深度值fin.open(path + "/depthmaps/scene_000.depth");ref_depth = cv::Mat(height, width, CV_64F);if (!fin) return false;for (int y = 0; y < height; y++)for (int x = 0; x < width; x++){double depth = 0;fin >> depth;ref_depth.ptr<double>(y)[x] = depth / 100.0;//？}return true;
}// 对整个深度图进行更新
bool update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {for (int x = boarder; x < width - boarder; x++)for (int y = boarder; y < height - boarder; y++){// 遍历每个像素if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散continue;// 在极线上搜索 (x,y) 的匹配Vector2d pt_curr;//pt_curr当前点Vector2d epipolar_direction;bool ret = epipolarSearch(ref,curr,T_C_R,Vector2d(x, y),depth.ptr<double>(y)[x],sqrt(depth_cov2.ptr<double>(y)[x]),pt_curr,epipolar_direction);if (ret == false) // 匹配失败continue;// 取消该注释以显示匹配//showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);// 匹配成功，更新深度图updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);}
}
/*********************************************主要算法**************************************************/
// 极线搜索
// 方法见书 12.2 12.3 两节
bool epipolarSearch(const Mat &ref, const Mat &curr,const SE3d &T_C_R, const Vector2d &pt_ref,const double &depth_mu, const double &depth_cov,//depth_mu:深度均值depth_cov深度方差Vector2d &pt_curr, Vector2d &epipolar_direction){Vector3d f_ref = px2cam(pt_ref);//像素坐标系转换成相机坐标系//cout<<"f_ref:"<<f_ref<<endl;f_ref.normalize();//归一化//cout<<"f_ref.normalize()"<<f_ref<<endl;Vector3d P_ref = f_ref * depth_mu; // 参考帧的 P 向量Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov;//(u-3q,u+3q)if (d_min < 0.1) d_min = 0.1;Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min)); // 按最小深度投影的像素Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max)); // 按最大深度投影的像素Vector2d epipolar_line = px_max_curr - px_min_curr; // 极线（线段形式）epipolar_direction = epipolar_line; // 极线方向epipolar_direction.normalize();//对所有数据进行归一化，相当于等比例缩小double half_length = 0.5 * epipolar_line.norm(); // 极线线段的半长度if (half_length > 100) half_length = 100; // 我们不希望搜索太多东西// 取消此句注释以显示极线（线段）//showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );// 在极线上搜索，以深度均值点为中心，左右各取半长度double best_ncc = -1.0;Vector2d best_px_curr;for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)Vector2d px_curr = px_mean_curr + l * epipolar_direction; // 待匹配点if (!inside(px_curr))continue;// 计算待匹配点与参考帧的 NCCdouble ncc = NCC(ref, curr, pt_ref, px_curr);//ref:参考图像curr:当前图像pt_ref:参考点px_curr:待匹配点if (ncc > best_ncc){best_ncc = ncc;best_px_curr = px_curr;}}if (best_ncc < 0.85f) // 只相信 NCC 很高的匹配return false;pt_curr = best_px_curr;return true;
}
/*****************************计算两张图像像素块的相关性***********************************************/
double NCC(const Mat &ref, const Mat &curr,const Vector2d &pt_ref, const Vector2d &pt_curr) {//ref:参考图像curr:当前图像pt_ref:参考点px_curr:待匹配点// 零均值-归一化互相关// 先算均值double mean_ref = 0, mean_curr = 0;vector<double> values_ref, values_curr; // 参考帧和当前帧的均值for (int x = -ncc_window_size; x <= ncc_window_size; x++)for (int y = -ncc_window_size; y <= ncc_window_size; y++){double value_ref = double(ref.ptr<uchar>(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0;mean_ref += value_ref;//依次累加窗口内的值，得到参考帧中极线上某个点所在块内的灰度值之和double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y));mean_curr += value_curr;//依次累加窗口内的值，得到当前帧中极线上的对应点所在块内的灰度值之和，注意这里用的是双线性插值法values_ref.push_back(value_ref);values_curr.push_back(value_curr);}//除以面积，得到均值mean_ref /= ncc_area;mean_curr /= ncc_area;// 计算 Zero mean NCC(公式13.12)double numerator = 0, demoniator1 = 0, demoniator2 = 0;for (int i = 0; i < values_ref.size(); i++) {double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);numerator += n;demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);}return numerator / sqrt(demoniator1 * demoniator2 + 1e-10); // 防止分母出现零
}
//更新深度滤波器
bool updateDepthFilter(const Vector2d &pt_ref,const Vector2d &pt_curr,const SE3d &T_C_R,const Vector2d &epipolar_direction,Mat &depth,//深度均值Mat &depth_cov2) //深度方差{// 不知道这段还有没有人看// 用三角化计算深度SE3d T_R_C = T_C_R.inverse();Vector3d f_ref = px2cam(pt_ref);//参考点像素转换为相机坐标系的坐标f_ref.normalize();//归一化处理Vector3d f_curr = px2cam(pt_curr);//当前点像素转换为相机坐标系的坐标f_curr.normalize();//归一化处理// 方程// d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC（视觉SLAM十四讲P156,公式7.24）// f2 = R_RC * f_cur// 转化成下面这个矩阵方程组// => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref] [f_ref^T t]// [ f_cur^T f_ref, -f2^T f2 ] [d_cur] = [f2^T t ]//重点：//s1*x1=s2*R*x2+t,移项得到s1*x1-s2*R*x2=t (1)//(1)两边同乘x1的转置x1T，得s1*x1T*x1-s2*x1T*R*x2=x1T*t (2) 注意"T"指的是转置的意思//因此接下来的A[]就是存放未知数为s1和s2的线性方程组的系数矩阵，b存放方程组右端的两个常数// cout<<"fref"<<f_ref<<endl;Vector3d t = T_R_C.translation();//cout<<"t"<<t<<endl;//cout<<"t.dot(f_ref)"<<t.dot(f_ref)<<endl;Vector3d f2 = T_R_C.so3() * f_curr;//AX=B;Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));Matrix2d A;A(0, 0) = f_ref.dot(f_ref);A(0, 1) = -f_ref.dot(f2);A(1, 0) = -A(0, 1);A(1, 1) = -f2.dot(f2);Vector2d ans = A.inverse() * b;Vector3d xm = ans[0] * f_ref; // ref 侧的结果Vector3d xn = t + ans[1] * f2; // cur 结果Vector3d p_esti = (xm + xn) / 2.0; // P的位置，取两者的平均double depth_estimation = p_esti.norm(); // 深度值// 计算不确定性（以一个像素为误差）Vector3d p = f_ref * depth_estimation;Vector3d a = p - t;//327公式13.7double t_norm = t.norm();double a_norm = a.norm();double alpha = acos(f_ref.dot(t) / t_norm);//a=arccos<p,t>double beta = acos(-a.dot(t) / (a_norm * t_norm));//beta=arccos<a,-t>Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);//f_curr_prime.normalize();double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);//计算beta'double gamma = M_PI - alpha - beta_prime;//计算y'double p_prime = t_norm * sin(beta_prime) / sin(gamma);//计算p‘的大小||p'||double d_cov = p_prime - depth_estimation;//计算sigma/obsdouble d_cov2 = d_cov * d_cov;//计算方差// 高斯融合//新的数据到来的深度估计均值以及方差double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];//具体高斯融合double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);//公式13.6double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);//融合后的高斯分布depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;return true;
}// 后面这些太简单我就不注释了（其实是因为懒）
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {imshow("depth_truth", depth_truth * 0.4);imshow("depth_estimate", depth_estimate * 0.4);imshow("depth_error", depth_truth - depth_estimate);waitKey(1);
}
//评估深度
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {double ave_depth_error = 0; // 平均误差double ave_depth_error_sq = 0; // 平方误差int cnt_depth_data = 0;for (int y = boarder; y < depth_truth.rows - boarder; y++)for (int x = boarder; x < depth_truth.cols - boarder; x++) {double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];ave_depth_error += error;ave_depth_error_sq += error * error;cnt_depth_data++;}ave_depth_error /= cnt_depth_data;ave_depth_error_sq /= cnt_depth_data;cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}
//显示极线匹配
void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) {Mat ref_show, curr_show;cv::cvtColor(ref, ref_show, CV_GRAY2BGR);cv::cvtColor(curr, curr_show, CV_GRAY2BGR);cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2);cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2);imshow("ref", ref_show);imshow("curr", curr_show);waitKey(1);
}
//画极线
void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,const Vector2d &px_max_curr) {Mat ref_show, curr_show;cv::cvtColor(ref, ref_show, CV_GRAY2BGR);cv::cvtColor(curr, curr_show, CV_GRAY2BGR);cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2);cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),Scalar(0, 255, 0), 1);imshow("ref", ref_show);imshow("curr", curr_show);waitKey(1);
}
结果还行：
小问题：
虚拟机bug：运行虚拟机时Pangolin的问题：terminate called after throwing an instance of ‘std::runtime_error‘
更多推荐
视觉SLAM之单目稠密重建
发布评论