专栏首页深度学习和计算机视觉SLAM程序阅读(第8讲 半稠密直接法)

SLAM程序阅读(第8讲 半稠密直接法)

这期我们来继续读一下半稠密直接法求解位姿的程序direct_semidense.cpp。

这个程序与我们上一期读的稀疏直接法direct_sparse.cpp的唯一差别就在于所选取的特征点,我们先来看一下程序的运行结果

稀疏法选取的是FAST特征点,特征点的选取会相对稀疏;在半稠密直接法中我们选点则是选取所有<灰度变化比较明显的像素>,将这些点作为特征点进行光流跟踪与直接法位姿求取。我们先来看一下子函数声明类、结构体的定义

struct Measurement
{
  ...
};
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
    ...
}
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
    ...
}
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
   ...
};

可以看出函数的声明与类的定义与稀疏直接法中的定义完全相同,也就是说两个程序在特征点位置与灰度信息的使用(结构体Measurement的定义)、坐标变换方式(project3Dto2D与project2Dto3D的定义)、使用g2o进行图优化求解位姿(poseEstimationDirect的声明与定义)都完全一致,因此在此不做赘述。

下面来看主函数。主函数中,可以发现除了第一帧特征点的选取方式有所改变之外,在读取图像位置信息、其余帧的直接法求解以及相应的函数调用、位姿求解结果的图像输出等其他操作也完全一致。那么我们就只来比对一下稀疏法与直接法在特征点求取上的不同。

稀疏直接法

if ( index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 去掉邻近边缘处的点
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }

首先实例化一个存储KeyPoint类对象的容器keypoints,用来存储提取得到的特征点。虽然后续有特征点的筛选(去掉边缘处的点),但由于筛选后的点会经过其他处理存入其他容器,因此在此没有使用链表。进而实例化一个Ptr结构体detector,调用detector内部的detect()函数进行FAST角点的计算,将当前帧color中的FAST角点存入容器keypoints中。

进而,通过遍历keypoints中每个元素(即刚提取出的每个特征点)的坐标值pt中的横纵像素值(即kp.pt.x与kp.pt.y),判断其像素坐标是否位于图像边缘处,以此来筛选掉边缘处的特征点。同时,判断在该像素位置的深度图中是否能查询到其深度信息,若查询不到该位置点的深度信息依然需要舍去该点。

最后,调用project2Dto3D()函数,利用特征点的像素坐标与深度值将其转化为相机坐标系下的3d坐标并存入三维向量p3d中,同时在灰度图中查找该点处的灰度值并存入浮点变量grayscale中,最后将这两个变量一同构造成一个Measurement结构体变量,压入之前定义好的容器measurements中。

半稠密直接法

if ( index ==0 )
        {
            // select the pixels with high gradiants 
            for ( int x=10; x<gray.cols-10; x++ )
                for ( int y=10; y<gray.rows-10; y++ )
                {
                    Eigen::Vector2d delta (
                        gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 
                        gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
                    );
                    if ( delta.norm() < 50 )
                        continue;
                    ushort d = depth.ptr<ushort> (y)[x];
                    if ( d==0 )
                        continue;
                    Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
                    float grayscale = float ( gray.ptr<uchar> (y) [x] );
                    measurements.push_back ( Measurement ( p3d, grayscale ) );
                }
            prev_color = color.clone();
            cout<<"add total "<<measurements.size()<<" measurements."<<endl;
            continue;
        }

在半稠密直接法中,通过遍历灰度图gray中的各个像素(在此做了一些约束,限制所查询的点的位置不在边缘处10像素范围内),构造了一个二维向量delta用来存储该点处的灰度值梯度(更确切得讲其实是每个像素位置横、纵两个方向相邻像素间灰度值变化值),即:

通过判断梯度向量的二范数(即欧氏距离)是否大于50来判断是否将其作为特征点。这里50这个判别值是因为选取norm()函数作为二范数求取函数,若选取其他函数如squaredNorm()则需要修改阈值。

最后,同样通过判断是否能够查询到深度值来选择保留该特征点与否,并最终将每个特征点的3d坐标与灰度值存入容器measurements。

下面来对比一下稀疏直接法与半稠密直接法的运行结果:

↑稀疏直接法

↑半稠密直接法

可以看出半稠密直接法的特征点个数更多,且就算随机选取五分之一的特征点进行展示,也可以遍布物体的边缘等灰度值突变的位置。

好了,本期的半稠密直接法程序阅读就到此结束。还是一样,由于近期我们开了矩阵分析以及其他多门学位课,小绿发文的周期会比较失调,希望大家能够不离不弃。感谢支持。

本文分享自微信公众号 - 小白学视觉(NoobCV)

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2018-11-23

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • ASP.NET Core使用Docker进行容器化托管和部署

    跟着阿笨一起玩NET
  • ASP.NET Core使用Docker-Compose实现多容器应用部署

    ASP.NET Core使用Docker-Compose容器编排实现多容器应用部署

    跟着阿笨一起玩NET
  • 【面试】必问的 Spring IOC,要看看了!!!

    IOC 和 DI 、DL 的关系(这个 DL,Avalon 和 EJB 就是使用的这种方式实现的 IoC):

    好好学java
  • Node.js 服务 Docker 容器化应用实践

    本篇不会讲解 Docker 命令的使用、安装等,因为在之前一篇文章 【一文零基础教你学会 Docker 入门到实践

    五月君
  • DevOps 系统的三个变迁

    把开发和运营作为整体来看待的DevOps工程思想在逐步深入人心,本文探讨了DevOps的起源和发展历程,从基于物理机/独立虚机的部署,到基于IaaS的部署,再到...

    Criss@陈磊
  • IDEA一键部署SpringBoot到Docker,这个骚操作你会了吗?

    IDEA是Java开发利器,springboot是Java生态中最流行的微服务框架,docker是时下最火的容器技术,那么它们结合在一起会产生什么化学反应呢?

    黄泽杰
  • FFmpeg里的Bitstream Filter

    ffmpeg -i input.mp4 -codec copy -bsf:v h264_mp4toannexb output.ts

    望天
  • 【Taro】363- 玩转 Taro 跨端之 flex 布局篇

    Taro 是一套遵循 React 语法规范的跨平台开发解决方案,但是目前当我们使用 Taro 的时候,在不同平台上的开发体验还有不一致的地方,所以我们也都期待有...

    pingan8787
  • 云原生计算对这三个热门市场的影响

    云原生计算可能是当今企业IT中最重要的趋势。从本质上讲,云原生技术将云计算的优势扩展到整个IT领域,其中包括内部部署技术和边缘计算。

    青果云小潘
  • ffmpeg常用命令及解释

    -c -c:v -c:a 设置音视频codec,帮助见ffmpeg -encoders

    望天

扫码关注云+社区

领取腾讯云代金券