Skip to content

Commit

Permalink
ch13/dense_monocular comments
Browse files Browse the repository at this point in the history
  • Loading branch information
gaoxiang12 committed Oct 13, 2016
1 parent d7d6dab commit f8eb4eb
Showing 1 changed file with 84 additions and 73 deletions.
157 changes: 84 additions & 73 deletions ch13/dense_monocular/dense_mapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,33 +19,38 @@ using namespace Eigen;

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 13.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题。
***********************************************/



// ------------------------------------------------------------------
// parameters
const int boarder = 20;
const int width = 640;
const int height = 480;
const double fx = 481.2f;
const int boarder = 20; // 边缘宽度
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 = 2;
const int ncc_area = (2*ncc_window_size+1)*(2*ncc_window_size+1);
const double min_cov = 0.1;
const double max_cov = 10;
const int ncc_window_size = 2; // 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; // 发散判定：最大方差

// ------------------------------------------------------------------
// core functions

// read the image files and poses from the REMODE dataset
// 重要的函数
// 从 REMODE 数据集读取数据
bool readDatasetFiles(
const string& path,
vector<string>& color_image_files,
vector<SE3>& poses
);

// update the depth and covariance with a new image
// in: gray-scale image (ref and current), pose of current to ref (T_C_R)
// out: depth and depth covariance
// 根据新的图像更新深度估计
bool update(
const Mat& ref,
const Mat& curr,
Expand All @@ -54,7 +59,7 @@ bool update(
Mat& depth_cov
);

// epipolar search in current image
// 极线搜索
bool epipolarSearch(
const Mat& ref,
const Mat& curr,
Expand All @@ -65,7 +70,7 @@ bool epipolarSearch(
Vector2d& pt_curr
);

// update the depth filter according to the epipolar search result
// 更新深度滤波器
bool updateDepthFilter(
const Vector2d& pt_ref,
const Vector2d& pt_curr,
Expand All @@ -74,15 +79,26 @@ bool updateDepthFilter(
Mat& depth_cov
);

// compute the NCC score
// 计算 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;
}

// ------------------------------------------------------------------
// some tool functions
// show the depth estimation
// 一些小工具
// 显示估计的深度图
bool plotDepth( const Mat& depth );

// pixel to camera transform
// 像素到相机坐标系
inline Vector3d px2cam ( const Vector2d px ) {
return Vector3d (
(px(0,0) - cx)/fx,
Expand All @@ -91,35 +107,24 @@ inline Vector3d px2cam ( const Vector2d px ) {
);
}

// camera to pixel transform
// 相机坐标系到像素
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
);
}

// check if a point is inside the image boarder
// 检测一个点是否在图像边框内
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;
}

// blinear interpolation in image
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;
}

// plot the epipolar match
// 显示极线匹配
void showEpipolarMatch( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_curr );

// plot the epipolar line
// 显示极线
void showEpipolarLine( const Mat& ref, const Mat& curr, const Vector2d& px_ref, const Vector2d& px_min_curr, const Vector2d& px_max_curr );
// ------------------------------------------------------------------

Expand All @@ -132,7 +137,7 @@ int main( int argc, char** argv )
return -1;
}

// read data from dataset
// 从数据集读取数据
vector<string> color_image_files;
vector<SE3> poses_TWC;
bool ret = readDatasetFiles( argv[1], color_image_files, poses_TWC );
Expand All @@ -143,24 +148,22 @@ int main( int argc, char** argv )
}
cout<<"read total "<<color_image_files.size()<<" files."<<endl;

// first image
// 第一张图
Mat ref = imread( color_image_files[0], 0 ); // gray-scale image
SE3 pose_ref_TWC = poses_TWC[0];
double init_depth = 3.0; // init values
double init_cov2 = 3.0; // init covariance
Mat depth( height, width, CV_64F, init_depth ); // depth estimation (in double)
Mat depth_cov( height, width, CV_64F, init_cov2 ); // depth covariance
double init_depth = 3.0; // 深度初始值
double init_cov2 = 3.0; // 方差初始值
Mat depth( height, width, CV_64F, init_depth ); // 深度图
Mat depth_cov( 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 ); // the i th image
Mat curr = imread( color_image_files[index], 0 );
if (curr.data == nullptr) continue;
SE3 pose_curr_TWC = poses_TWC[index];
SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // T_C_W * T_W_R = T_C_R
boost::timer timer;
SE3 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_cov );
cout<<"update costs time: "<<timer.elapsed()<<endl;
plotDepth( depth );
imshow("image", curr);
waitKey(1);
Expand All @@ -184,6 +187,7 @@ bool readDatasetFiles(

while ( !fin.eof() )
{
// 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
string image;
fin>>image;
double data[7];
Expand All @@ -199,16 +203,18 @@ bool readDatasetFiles(
return true;
}

// 对整个深度图进行更新
bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov )
{
#pragma omp parallel for
for ( int x=boarder; x<width-boarder; x++ )
#pragma omp parallel for
for ( int y=boarder; y<height-boarder; y++ )
{
if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov )
// 遍历每个像素
if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收敛或发散
continue;
// search the epipolar match of point (x,y)
// 在极线上搜索 (x,y) 的匹配
Vector2d pt_curr;
bool ret = epipolarSearch (
ref,
Expand All @@ -220,17 +226,19 @@ bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat&
pt_curr
);

if ( ret == false ) // epipolar search failed
if ( ret == false ) // 匹配失败
continue;

// 取消该注释以显示匹配
// showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr );

// if epipolar search succeeded, update the depth estimation
// 匹配成功，更新深度图
updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov );
}
}


// 极线搜索
// 方法见书 13.2 13.3 两节
bool epipolarSearch(
const Mat& ref, const Mat& curr,
const SE3& T_C_R, const Vector2d& pt_ref,
Expand All @@ -239,39 +247,40 @@ bool epipolarSearch(
{
Vector3d f_ref = px2cam( pt_ref );
f_ref.normalize();
Vector3d P_ref = f_ref*depth_mu;
Vector3d P_ref = f_ref*depth_mu; // 参考帧的 P 向量

Vector2d px_mean_curr = cam2px( T_C_R*P_ref );
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;
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 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;
Vector2d epipolar_direction = epipolar_line;
Vector2d epipolar_line = px_max_curr - px_min_curr; // 极线（线段形式）
Vector2d epipolar_direction = epipolar_line; // 极线方向
epipolar_direction.normalize();
double half_length = 0.5*epipolar_line.norm();
if ( half_length>100 ) half_length = 100; // prevent too large epipolar line
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 );

// search the epipolar line
// 在极线上搜索，以深度均值点为中心，左右各取半长度
double best_ncc = -1.0;
Vector2d best_px_curr;
for ( double l=-half_length; l<=half_length; l+=0.7 ) // l+=sqrt(2) is good
for ( double l=-half_length; l<=half_length; l+=0.7 ) // l+=sqrt(2)
{
Vector2d px_curr = px_mean_curr + l*epipolar_direction;
Vector2d px_curr = px_mean_curr + l*epipolar_direction; // 待匹配点
if ( !inside(px_curr) )
continue;
// compute the NCC score
// 计算待匹配点与参考帧的 NCC
double ncc = NCC( ref, curr, pt_ref, px_curr );
if ( ncc>best_ncc )
{
best_ncc = ncc;
best_px_curr = px_curr;
}
}
if ( best_ncc < 0.85f ) // only trust high ncc matches
if ( best_ncc < 0.85f ) // 只相信 NCC 很高的匹配
return false;
pt_curr = best_px_curr;
return true;
Expand All @@ -282,10 +291,10 @@ double NCC (
const Vector2d& pt_ref, const Vector2d& pt_curr
)
{
// Zero-mean Normalized Cross Correlation
// means
// 零均值-归一化互相关
// 先算均值
double mean_ref = 0, mean_curr = 0;
vector<double> values_ref, values_curr;
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++ )
{
Expand All @@ -302,6 +311,7 @@ double NCC (
mean_ref /= ncc_area;
mean_curr /= ncc_area;

// 计算 Zero mean NCC
double numerator = 0, demoniator1 = 0, demoniator2 = 0;
for ( int i=0; i<values_ref.size(); i++ )
{
Expand All @@ -310,7 +320,7 @@ double NCC (
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 ); // avoid zero demoniator
return numerator / sqrt( demoniator1*demoniator2+1e-10 ); // 防止分母出现零
}

bool updateDepthFilter(
Expand All @@ -323,13 +333,14 @@ bool updateDepthFilter(
{
// 我是一只喵
// 不知道这段还有没有人看
// triangluate the point to compute depth
// 用三角化计算深度
SE3 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();

// 方程参照本书第 7 讲三角化一节
Vector3d t = T_R_C.translation();
Vector3d f2 = T_R_C.rotation_matrix() * f_curr;
Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );
Expand All @@ -344,10 +355,10 @@ bool updateDepthFilter(
-A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
Vector3d xm = lambdavec ( 0,0 ) * f_ref;
Vector3d xn = t + lambdavec ( 1,0 ) * f2;
Vector3d d_esti = ( xm+xn ) / 2.0; // the estimated vector by triangulation
double depth_estimation = d_esti.norm(); // depth value
Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量
double depth_estimation = d_esti.norm(); // 深度值

// compute the covariance
// 计算不确定性（以一个像素为误差）
Vector3d p = f_ref*depth_estimation;
Vector3d a = p - t;
double t_norm = t.norm();
Expand All @@ -360,7 +371,7 @@ bool updateDepthFilter(
double d_cov = p_prime - depth_estimation;
double d_cov2 = d_cov*d_cov;

// Gaussian fusion
// 高斯融合
double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];
double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ];

Expand All @@ -373,7 +384,7 @@ bool updateDepthFilter(
return true;
}


// 后面这些太简单我就不注释了（其实是因为懒）
bool plotDepth(const Mat& depth)
{
imshow( "depth", depth*0.4 );
Expand Down

0 comments on commit f8eb4eb

Please sign in to comment.