建图学习一

最新推荐文章于 2023-04-03 17:59:53 发布

磊磊哈哈

最新推荐文章于 2023-04-03 17:59:53 发布

阅读量116

点赞数

原文链接：https://editor.csdn.net/md?articleId=111356530

版权

SLAM十四讲专栏收录该内容

37 篇文章 8 订阅

订阅专栏

使用目的：定位、导航、避障、重建、交互
类型：稠密、稀疏

概念

来自slam14将

求取每一个像素点对应距离：
(1):单目相机：估计相机运动，三角化计算像素距离
(2):双目相机：左右目视差计算像素距离
(3):RGB：直接获取
立体视觉
单目/双目获得像素与实际物理距离方法
移动视角的立体视觉（Moving View Stereo Vision）
移动单目相机
确定某像素出现在其他图里位置

极线搜索

$d:像素p_1对应的深度$
$p_2:与p_1相匹配的特征点$
在这里插入图片描述
问题：
对于第一个视角生成的图像 $p_1,p_2$ 是不确定的， $p_1$ 对应空间点在第二个视角的图像中是一条直线（这条直线称之为极线），如何在这条直线上确定与 $p_1$ 对应 $p_2$ 的方法称之为块匹配( $p_1$ 与 $p_2$ 周围的小块灰度不变，求取两个像素周围小块灰度之间的差)。
小块间差异计算方法：
（1）：SAD（Sum of absolute diference）:两个小块绝对值之和：
$S(A,B)_{SAD}=\sum_{i,j}|A(i,j)-B(i,j)|(0:表示相似 1：表示不相似)$
（2）：SSD(Sum of Squared Distance):平方和
$S(A,B)_{SSD}=\sum_{i,j}(A(i,j)-B(i,j))^2(0:表示相似 1：表示不相似)$
（3）：NCC(Normalized Cross Corelation):归一化互关性
$S(A,B)_{NCC}=\frac{\sum_{i,j}A(i,j)B(i,j)}{\sqrt{\sum_{i,j}A(i,j)^2\sum_{i,j}B(i,j)^2}}(1:表示相似 0：表示不相似)$
（4）：误差来源：
几何视差误差:主要是由于计算的位姿和投影方程过程中产生的误差造成的
光度视差误差:主要是图像噪声导致图像灰度值的变化，这会导致最后在极线上找到的和关键帧上点的灰度值差异最小（SSD误差最小）的匹配点并不一定是真正的匹配点
在LSD还会有不确定估计

深度滤波器

在极线上计算小块间的差异，得到了匹配得分在极线上的分布。

假设某个图像点深度d服从：
$P(d)=N(u,\sigma^2)$
新的数据点也是一个高斯分布：
$P(d_{obs})=N(u_{d_{obs}},\sigma_{obs}^2)(计算方式很多：几何不确定度，光度不确定度)$
使用新的数据点更新原来的数据，得到新的高斯分布：
$P(d_{new})=N(\frac{\sigma_{obs}^2 u+\sigma^2u_{obs}}{\sigma^2+\sigma_{obs}^2},\sigma^2_{fuse}=\frac{\sigma^2\sigma^2_{obs}}{\sigma^2+\sigma_{obs}^2})$
如何计算 $\sigma_{obs}$
(1)：已知
$\overrightarrow a=\overrightarrow a-\overrightarrow t$
$\overrightarrow \alpha=arccos<\overrightarrow p,\overrightarrow t>$
$\overrightarrow \beta=arccos<\overrightarrow a,-\overrightarrow t>$
(2)：对 $p_2$ 添加一个扰动，使 $\beta$ 产生一个变化得到 $\beta'$ ,可以得到：
$\beta'=arcos<\overrightarrow{O_2p'_2},-\overrightarrow t>$
$\gamma=\pi-\alpha-\beta'$
(3)：求得 $p^{'}$ 的大小为：
$||p'||=||t||\frac{sin \beta'}{sin \gamma}$
(4)：确定 $\sigma=||p||-||p'||$ （当 $\sigma$ 极线搜索的不确定度，仅为一个像素）
估计稠密度一个完整过程
（1）：假设所有像素的深度满足某个初始的高斯分布
（2）：新数据产生时，通过极线搜索和块匹配确定投影点位置
（3）：根据几何误差计算三角化后的深度及不确定度
（4）：融合当前数据进上一次估计中，满足条件停止，不满足返回第2步。

slam14讲测试数据集下载

wget http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip

代码阅读：

克莱目法则
已知： $\begin{cases}ax+by=e\\ cx+dy=f\end{cases}$
当 $\left[\begin{matrix}a&b\\c&d\end{matrix}\right]可逆：$
可以得到： $x=\frac{\left|\begin{matrix}e&f\\b&d\end{matrix}\right|}{\left|\begin{matrix}a&b\\c&d\end{matrix}\right|}=\frac{ed-bf}{ad-bc}\\ y=\frac{\left|\begin{matrix}a&e\\c&f\end{matrix}\right|}{\left|\begin{matrix}a&e\\c&f\end{matrix}\right|}=\frac{af-ec}{ad-bc}$
完整代码及注释

#include <vector>
#include <iostream>
#include <fstream>
#include <sophus/se3.hpp>
#include <opencv/cv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
using namespace cv;
using namespace Sophus;
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;
//@param    px   像素坐标转换为3d坐标
inline Vector3d px2cam(const Vector2d px){
    return Vector3d((px(0,0)-cx)/fx,(px(1,0)-cy)/fy,1);
}
//@param    p_cam   3d坐标转换为像素坐标
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);
}

//双线性插值
//@param    img     图片
//@param    pt      像素坐标
inline double  doublelineinsert(const Mat &img,const Vector2d &pt){
    uchar *d=&img.data[int(pt(1,0))*img.step+int(pt(0,0))];//y,x 读取图像最左上角的参数
    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;
}
const int boarder = 20;         // 边缘宽度
//检测点是否在图像边框内
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);
}
//读取相机初始数据
//@param filename           路径
//@param color_image_files  图片位置
//@param pose               姿态
//@param ref_depth          深度图
bool ReadDateFiles(const string &filename,vector<string> &color_image_files,
vector<SE3d> &pose,Mat &ref_depth){
    ifstream fin(filename+"/first_200_frames_traj_over_table_input_sequence.txt");
    
    if(!fin)return false;
    while(!fin.eof()){
        double data[7];      
        string image_name;//图片名
        fin>>image_name;
        for(double &d:data)fin>>d;
        color_image_files.push_back(filename+"/images/"+image_name);
        pose.push_back(SE3d(Quaterniond(data[6],data[3],data[4],data[5]),Vector3d(data[0],data[1],data[2])));//转化为SE3d
        if (!fin.good()) break;
    }
    fin.close();
    fin.open(filename + "/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;
        }
}
//显示极线
//@param ref            起始帧
//@param curr           当前帧
//@param px_ref         起始帧像素
//@param px_min_ref     极线搜索最小范围深度
//@param px_max_xurr    极线搜索大范围深度
void showEpipolarline(const Mat &ref,const Mat &curr,const Vector2d &px_ref,
                    const Vector2d &px_min_ref,const Vector2d &px_max_curr){
    Mat ref_show,curr_show;
    cvtColor(ref,ref_show,CV_GRAY2BGR);
    cvtColor(curr,curr_show,CV_GRAY2BGR);

    cv::circle(ref_show,Point2f(px_ref(0,0),px_ref(1,0)),5,cv::Scalar(0,255,0),2);
    //图像，中心点，半径，颜色，宽度
    cv::circle(curr_show,Point2f(px_min_ref(0,0),px_min_ref(1,0)),5,cv::Scalar(0,255,0),2);
    cv::circle(curr_show,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_ref(0, 0), px_min_ref(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);
}
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // 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){
        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=doublelineinsert(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;
        //cout<<"mean_curr"<<mean_curr<<endl;
        //cout<<"mean_curr"<<mean_curr<<endl;
        //计算评分
        //先减去均值
        double AB=0,A=0,B=0;
        for(int i=0;i<values_ref.size();i++){
            AB+=(values_ref[i]-mean_ref)*(values_curr[i]-mean_curr);
            A+=((values_ref[i]-mean_ref)*(values_ref[i]-mean_ref));
            B+=((values_curr[i]-mean_curr)*(values_curr[i]-mean_curr));
        }
        return (AB/sqrt(A*B+1e-10));//1e-10 防止出现分母为零的无效值

}
//极线搜索
//@param  ref                   起始帧
//@param  curr                  当前帧
//@param  T_C_R                 两帧之间的转换矩阵 SE3d类型
//@param  pt_ref                参考图像中点的位置       
//@param  depth_mu              深度均值
//@param  depth_cov             深度方差
//@param  pt_curr               当前点
//@param  epipolar_direction    极线方向
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){
    Vector3d f_ref=px2cam(pt_ref);  //图像坐标系转化为归一化坐标系
    f_ref.normalize();              //归一化
    Vector3d P_ref=f_ref*depth_mu;  //参考坐标=归一化位置*深度均值
    
    Vector2d px_mean_curr=cam2px(T_C_R*P_ref);//按深度均值投影的像素

    //确定最小深度值和最大深度值，两个点连线在第二帧的投影即为所要搜寻的极线范围
    //这里假设深度值服从高斯分布，所以以均值为中心，左右各取3(sigma：方差)，在当前帧求投影
    double d_min=depth_mu-3*depth_cov,d_max=depth_mu+3*depth_cov;//定义当前位置的深度最小值=深度均值-3×深度方差，最大值=深度均值+3×深度方差
    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_cur;
    for(double l=-half_length;l<=half_length;l+=0.7){
        Vector2d px_curr=px_mean_curr+l*epipolar_direction;//获取极线上的一点
        if(!inside(px_curr))//如果不再范围内跳过
            continue;
        double ncc=NCC(ref,curr,pt_ref,px_curr);
        if(ncc>best_ncc){
            best_ncc=ncc;
            best_px_cur=px_curr;
        }
    }
    if(best_ncc<0.85)
        return false;
    pt_curr = best_px_cur;
    return true;
};
//显示匹配
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);
}
//更新深度滤波器
//@param    pt_ref              初始帧图像参考点
//@param    pt_curr             当前帧图像参考点
//@param    T_C_R               姿态转换角
//@param    epipolar_direction  极线方向
//@param    depth               深度均值
//@param    depth_cov2          深度均值方差
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();
        //原式：S1*X1=S2*R*X2+t  
        //移项：S1*X1-S2*R*X2=t
        //移项两边同乘：x1_T(x1的转置)  S1*x1_T*X1-S2*x1_T*R*X2=x1_T*t
        //移项两边同乘：(R*X2)_T       S1*(R*X2)_T*X1-S2*(R*X2)_T*R*X2=(R*X2)_T*t
        //已知：R=T_R_C.so3();T_R_C.translation();X1=f_ref; X2=f_curr;
        //f2=(R*X2)
        //再利用克莱姆法则求解S1,S2
        Vector3d t=T_R_C.translation();//获取平移部分
        Vector3d f2=T_R_C.so3()*f_curr;//旋转部分
        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)=f_ref.dot(f2);
        A(1,1)=-f2.dot(f2);
        Vector2d ans=A.inverse()*b;
        Vector3d xm=ans[0]*f_ref;//使用起始帧的推测深度结果
        Vector3d xn=t+ans[1]*f2;//当前帧转换的起始帧的推测结果
        Vector3d p_esti=(xm+xn)/2;
        double depth_estimation=p_esti.norm();//求得深度估计值
        // cout<<"depth_estimation"<<depth_estimation<<endl;
        //计算不确定度,书中313页
        //a=p-t
        Vector3d p=f_ref*depth_estimation;
        Vector3d a=p-t;
        double a_norm=a.norm();
        double t_norm=t.norm();
        double alpha=acos(f_ref.dot(t)/t_norm);//这里f_ref已经归一化 模值为1
        double beta=acos(a.dot(-t)/(a_norm*t_norm));
        Vector3d f_curr_prime=px2cam(pt_curr+epipolar_direction);//pp'向量
        f_curr_prime.normalize();//归一化
        double bete_prime=acos(f_curr_prime.dot(-t)/t_norm);//beta'  
        double gamma=M_PI-alpha-bete_prime;
        double p_prime_norm=t_norm*sin(bete_prime)/sin(gamma);
        double d_cov=p_prime_norm-depth_estimation;
        double d_cov2=d_cov*d_cov;
        //  cout<<"d_cov"<<d_cov<<endl;
        //高斯融合
        double u=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 u_fuse=(d_cov2*u+sigma2*depth_estimation)/(sigma2+d_cov2);
        double sigma_fuse2=(d_cov2*sigma2)/(sigma2+d_cov2);//更新高斯分布
        // cout<<"u_fuse"<<u_fuse<<endl;
        // cout<<"sigma_fuse2"<<sigma_fuse2<<endl;
        depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = u_fuse;
        depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;
        return true;
}   


const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差
//更新深度图
//@param Mat            第一帧图像
//@param curr           当前图像
//@param pose_insect    两幅图像的夹角
//@param depth          深度图
//@param depth_cov2     深度方差
bool update(const Mat &ref,const Mat &curr,const SE3d &pose_insect,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;//深度收敛或者发散跳过
            Vector2d pt_curr;
            Vector2d epipolar_direction;
            bool ret=epipolarSearch(//计算对应点的像素坐标
                ref,
                curr,
                pose_insect,
                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,pose_insect,epipolar_direction,depth,depth_cov2);
        }
    }
};

//评测深度估计准确度    实际值与估计值之间的平均误差
//@param depth_truth        实际值
//@param depth_estimate     估计值
void evaludateDepth(const Mat &depth_truth,const Mat &depth_estimate){
    double aver_depth_error=0;      //平均误差
    double aver_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];//实际值-深度值
            aver_depth_error +=error;
            aver_depth_error_sq+=error*error;
            cnt_depth_data++;
        }
    }
    aver_depth_error /=cnt_depth_data;
    aver_depth_error_sq /=cnt_depth_data;
    cout << "Average squared error = " << aver_depth_error_sq << ", average error: " << aver_depth_error << endl;
}

// 画深度图
//@param  depth_truth           实际值
//@param  depth_estimate        估计值
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);
}


int main(int argc, char const *argv[])
{
    vector<string> image_files;vector<SE3d> pose;Mat ref_depth;
    const string filename="/home/n1/notes/map/test_data";
    ReadDateFiles(filename,image_files,pose,ref_depth);
    
    Mat ref=imread(image_files[0],0);//读取第一副图
    SE3d pose_ref=pose[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图 全部赋值为3
    Mat depth_cov2(height,width,CV_64F,init_cov2);            // 深度图 全部赋值为3

    for(int index=1;index<image_files.size();index++){
        cout<<"*** loop"<<index<<"***"<<endl;
        Mat curr=imread(image_files[index],0);//读取图片
        if(curr.data==nullptr)continue;
        SE3d pose_curr_TWC=pose[index];
        SE3d pose_insect=pose_curr_TWC.inverse()*pose_ref;//求夹角
        update(ref,curr,pose_insect,depth,depth_cov2);  //更新深度 第一帧，当前帧，当前姿态夹角，深度图，深度方差
        evaludateDepth(ref_depth,depth);                //计算深度误差
        double ncc=NCC(ref,curr,Vector2d(height/2,width/2),Vector2d(height/2,width/2));
        plotDepth(ref_depth,depth);
        imshow("image", curr);
        waitKey(1);
        // cout<<"ncc"<<ncc<<endl;
        // Vector3d RPY=pose_insect.rotationMatrix().eulerAngles(0,1,2);
        // cout<<"angle:X:"<<RPY[0]<<"Y:"<<RPY[1]<<"Z:"<<RPY[2]<<endl;
    }
    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;
    return 0;
}