【从零开始写视觉SLAM】v0.1基于特征点的简单VO

news2024/11/18 12:40:09

v0.1版本的oSLAM实现了基于orb特征点的简单视觉里程计，通过连续两帧的rgbd数据实现相机相对位姿的估计。
读取RGBD数据
orb特征点提取
PnP/ICP位姿估计

这部分理论上相对简单一点，咱们就直接上实现。

  1. VisualOdometer类

VisualOdometer.hpp

#pragma once
#include <vector>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "Frame.hpp"

namespace oSLAM
{
    class VisualOdometer
    {
    private:
        std::vector<Frame> frames;
        int max_key_points_num;
        double cx, cy, fx, fy;
        double depth_scale;
        std::vector<cv::DMatch> matches;

        void feature_extract(const cv::Mat& rgb, Frame& frame);
        void calc_depth(const cv::Mat& depth, Frame& frame);
        void feature_match(const Frame& ref, const Frame& cur, std::vector<cv::DMatch>& matches);
        void calc_pose_relative(const Frame& ref, Frame& cur, const std::vector<cv::DMatch>& matches);
        void pose_estimation_3d2d(const std::vector<cv::Point3d> &pts1, const std::vector<cv::Point2d> &pts2, cv::Mat &R, cv::Mat &t);
        void pose_estimation_3d3d(const std::vector<cv::Point3d> &pts1, const std::vector<cv::Point3d> &pts2, cv::Mat &R, cv::Mat &t);
    public:
        void add(double timestamp, const cv::Mat &rgb, const cv::Mat& depth);
        void set_pose(int frame_idx, const cv::Mat& R, const cv::Mat& T);
        void get_pose(int frame_idx, cv::Mat& R, cv::Mat& T);
        void get_3d_points(int frame_idx, std::vector<cv::Point3d> &key_points_3d);
        
        VisualOdometer(int max_key_points_num, double cx, double cy, double fx, double fy, double depth_scale);
        ~VisualOdometer();
    };
}

VisualOdometer.cpp

#include "VisualOdometer.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>

using namespace oSLAM;
using namespace std;
using namespace cv;

VisualOdometer::VisualOdometer(int max_key_points_num, double cx, double cy, double fx, double fy, double depth_scale)
{
    VisualOdometer::max_key_points_num = max_key_points_num;
    VisualOdometer::cx = cx;
    VisualOdometer::cy = cy;
    VisualOdometer::fx = fx;
    VisualOdometer::fy = fy;
    VisualOdometer::depth_scale = depth_scale;
}

VisualOdometer::~VisualOdometer()
{
}

void VisualOdometer::feature_extract(const cv::Mat &rgb, Frame &frame)
{
    Ptr<ORB> orb_detector = ORB::create(max_key_points_num);
    orb_detector->detect(rgb, frame.key_points);
    orb_detector->compute(rgb, frame.key_points, frame.descriptors);
}

void VisualOdometer::calc_depth(const cv::Mat &depth, Frame &frame)
{
    for (int i=0;i<frame.key_points.size();i++)
    {
        double x = frame.key_points[i].pt.x;
        double y = frame.key_points[i].pt.y;

        double dis = depth.at<uint16_t>(int(y),int(x)) / depth_scale;
        frame.key_points_3d.push_back(Point3d((x-cx)/fx*dis, (y-cy)/fy*dis, dis));
    }
}

void VisualOdometer::pose_estimation_3d2d(const std::vector<cv::Point3d> &pts1, const std::vector<cv::Point2d> &pts2, cv::Mat &R, cv::Mat &t)
{
    // 利用PnP求解位姿初值
    Mat K = (Mat_<double>(3,3) << fx, 0, cx, 
        0, fy, cy,
        0, 0, 1);
    
    Mat rvec, tvec;
    solvePnPRansac(pts1, pts2, K, Mat::zeros(1, 5, CV_64FC1), rvec, tvec);

    Rodrigues(rvec, R);
    t = (Mat_<double>(3,1) << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));

    // 优化位姿和3D点坐标
    // ToDo
}


void VisualOdometer::pose_estimation_3d3d(const std::vector<cv::Point3d> &pts1, const std::vector<cv::Point3d> &pts2, cv::Mat &R, cv::Mat &t)
{
    Point3d p1(0, 0, 0), p2(0, 0, 0); // center of mass
    int N = pts1.size();
    for (int i = 0; i < N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3d(Vec3d(p1) / N);
    p2 = Point3d(Vec3d(p2) / N);
    vector<Point3d> q1(N), q2(N); // remove the center
    for (int i = 0; i < N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++)
    {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    Eigen::Matrix3d R_ = U * (V.transpose());
    if (R_.determinant() < 0)
    {
        R_ = -R_;
    }
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::Mat
    R = (Mat_<double>(3, 3) << R_(0, 0), R_(0, 1), R_(0, 2),
         R_(1, 0), R_(1, 1), R_(1, 2),
         R_(2, 0), R_(2, 1), R_(2, 2));
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

void VisualOdometer::calc_pose_relative(const Frame& ref, Frame& cur, const std::vector<cv::DMatch>& matches)
{
    vector<Point3d> ref_key_points_3d, cur_key_points_3d;
    vector<Point2d> ref_key_points_2d, cur_key_points_2d;

    // 筛选3D点
    for(auto match : matches)
    {
        Point3d ref_key_point_3d = ref.key_points_3d[match.queryIdx];
        Point3d cur_key_point_3d = cur.key_points_3d[match.trainIdx];

        if (ref_key_point_3d.z == 0 || cur_key_point_3d.z == 0)
        {
            continue;
        }

        ref_key_points_3d.push_back(ref_key_point_3d);
        cur_key_points_3d.push_back(cur_key_point_3d);

        ref_key_points_2d.push_back(ref.key_points[match.queryIdx].pt);
        cur_key_points_2d.push_back(cur.key_points[match.trainIdx].pt);
    }

    // 3D点计算位姿
    Mat R, T;
    //pose_estimation_3d3d(cur_key_points_3d, ref_key_points_3d, R, T);
    pose_estimation_3d2d(ref_key_points_3d, cur_key_points_2d, R, T);

    cur.R = R * ref.R;
    cur.T = R * ref.T + T;
}

void VisualOdometer::feature_match(const Frame& ref, const Frame& cur, std::vector<cv::DMatch>& matches)
{
    vector<DMatch> initial_matches;
    BFMatcher matcher(NORM_HAMMING);

    matcher.match(ref.descriptors, cur.descriptors, initial_matches);

    double min_dis = initial_matches[0].distance;
    for(auto match : initial_matches)
    {
        if (match.distance < min_dis)
            min_dis = match.distance;
    }

    matches.clear();
    for(auto match : initial_matches)
    {
        if (match.distance <= MAX(min_dis * 2, 30))
            matches.push_back(match);
    }
}

void VisualOdometer::add(double timestamp, const Mat &rgb, const Mat &depth)
{
    Frame frame;
    frame.time_stamp = timestamp;
    frame.rgb = rgb.clone();
    frame.depth = depth.clone();

    // 提取rgb图像的orb特征点
    VisualOdometer::feature_extract(rgb, frame);

    // 提取关键点的深度信息
    VisualOdometer::calc_depth(depth, frame);

    // 如果不是第一帧
    if (VisualOdometer::frames.size() == 0)
    {
        frame.R = Mat::eye(3,3,CV_64FC1);
        frame.T = Mat::zeros(3,1,CV_64FC1);
    }
    else
    {
        // 当前帧与上一帧特征点匹配
        VisualOdometer::feature_match(
            VisualOdometer::frames[VisualOdometer::frames.size()-1], 
            frame,
            VisualOdometer::matches);

        // 计算相对位姿关系
        VisualOdometer::calc_pose_relative(
            VisualOdometer::frames[VisualOdometer::frames.size()-1], 
            frame,
            VisualOdometer::matches);
    }
    
    // 将当前帧加入队列
    VisualOdometer::frames.push_back(frame);
}

void VisualOdometer::get_pose(int frame_idx, Mat &R, Mat &T)
{
    if (VisualOdometer::frames.size() <= abs(frame_idx))
    {
        R = Mat();
        T = Mat();
        return;
    }
    else
    {
        if (frame_idx >= 0)
        {
            R = VisualOdometer::frames[frame_idx].R.clone();
            T = VisualOdometer::frames[frame_idx].T.clone();
        }
        else
        {
            R = VisualOdometer::frames[VisualOdometer::frames.size() + frame_idx].R.clone();
            T = VisualOdometer::frames[VisualOdometer::frames.size() + frame_idx].T.clone();
        }
    }
}

void VisualOdometer::set_pose(int frame_idx, const cv::Mat& R, const cv::Mat& T)
{
    if (VisualOdometer::frames.size() <= abs(frame_idx))
    {
        return;
    }
    else
    {
        if (frame_idx >= 0)
        {
            VisualOdometer::frames[frame_idx].R = R.clone();
            VisualOdometer::frames[frame_idx].T = T.clone();
        }
        else
        {
            VisualOdometer::frames[VisualOdometer::frames.size() + frame_idx].R = R.clone();
            VisualOdometer::frames[VisualOdometer::frames.size() + frame_idx].T = T.clone();
        }
    }
}

void VisualOdometer::get_3d_points(int frame_idx, std::vector<cv::Point3d> &key_points_3d)
{
    if (VisualOdometer::frames.size() <= abs(frame_idx))
    {
        key_points_3d.clear();
        return;
    }
    else
    {
        if (frame_idx >= 0)
        {
            key_points_3d = VisualOdometer::frames[frame_idx].key_points_3d;
        }
        else
        {
            key_points_3d = VisualOdometer::frames[VisualOdometer::frames.size() + frame_idx].key_points_3d;
        }
    }
}
  1. Frame类
#pragma once
#include <vector>
#include <opencv2/opencv.hpp>

namespace oSLAM
{
    class Frame
    {
    public:
        std::vector<cv::KeyPoint> key_points;
        cv::Mat descriptors;
        std::vector<cv::Point3d> key_points_3d;
        cv::Mat R;
        cv::Mat T;
        cv::Mat rgb;
        cv::Mat depth;
        double time_stamp;
    };
}

最终在rgbd_dataset_freiburg2_desk数据集上测试类一下效果，感觉有点拉，跑着跑着就飘了（红色的是真值，绿色的是计算结果），等实现了完整的vo在回来分析一下。
在这里插入图片描述
结果展示使用了Pangolin和yuntianli91的pangolin_tutorial

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/541880.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

MySQL数据库---笔记1

MySQL数据库---笔记1

MySQL数据库---笔记1 一、数据库概述1.1、什么是数据库1.2、数据库的安装与启动1.3、MySQL数据模型 二、SQL2.1、通用语法及分类2.2、DDL2.2.1、数据库操作 一、数据库概述 1.1、什么是数据库 名称全称简称数据库存储数据的仓库&#xff0c;数据是有组织的进行存储DataBase (…
阅读更多...
LeetCode每日一题之二分搜索

LeetCode每日一题之二分搜索

文章目录 1.关于二分搜索常见的误区2.左闭右闭区间的写法3.左闭右开区间的写法4.找到第一个大于target的数5.找到第一个小于target的数6.找到第一个大于等于taregt的数7.找到第一个小于等于target的数 1.关于二分搜索常见的误区 区间的定义&#xff1a; 2.左闭右闭区间的写法…
阅读更多...
（MIT6.045）自动机、可计算性和复杂性-正则表达式

（MIT6.045）自动机、可计算性和复杂性-正则表达式

语言(language)的计算性质&#xff1a;交、并、补、反转、拼接、星号&#xff08;*&#xff09; 星号是一元运算符&#xff0c;表示一个语言和自己的有穷次笛卡尔积。 回顾&#xff1a;正则语言&#xff08;Regular Language&#xff09;指可以用DFA描述的语言。 正则表达式…
阅读更多...
Linux内核panic简析

Linux内核panic简析

源码基于&#xff1a;Linux 5.4 0. 前言 内核异常的级别大致分为三个&#xff1a;BUG、oops、panic。 BUG 是指那些不符合内核的正常设计&#xff0c;但内核能够检测出来并且对系统运行不会产生影响的问题&#xff0c;比如在原子上下文中休眠&#xff0c;在内核中用 BUG 标识。…
阅读更多...
Linux——线程3|线程互斥和同步

Linux——线程3|线程互斥和同步

加锁保护 我们上一篇提到过，多个线程执行下面代码可能会出错，具体原因可查看上一篇Linux博客。
阅读更多...
Vue3相关知识点笔记（持续更新中。。。。）

Vue3相关知识点笔记（持续更新中。。。。）

目录标题 1、Vue3中的组合式Api有哪些? 和Vue2的Options Api有什么不同?Vue3中的组合式API主要包括以下几个&#xff1a;与Vue2的option Api有什么不同 2、Vue3.0的设计目标是什么&#xff1f;做了哪些优化&#xff1f;设计目标是什么&#xff1f;哪些优化&#xff1f; 3、Vu…
阅读更多...
python：随机森林分类器的性能评估（决策树数量的影响）

python：随机森林分类器的性能评估（决策树数量的影响）

作者：CSDN @ _养乐多_ 随机森林(Random Forest)是一种强大的机器学习算法，常用于分类和回归任务。它由多个决策树构成，通过集成学习的方式进行预测。在本篇博客中，我们将探讨随机森林分类器在不同决策树数量下的性能，并绘制相应的图表进行可视化分析。OOB误差，0被误判为…
阅读更多...
C++ 实现堆排序

C++ 实现堆排序

时空复杂度 时间复杂度 排序复杂度 O ( n l o g n ) O(nlogn) O(nlogn) 建堆复杂度 O ( n ) O(n) O(n) 空间复杂度 由于堆排序是一种就地设计的排序算法&#xff0c;空间需求是恒定的&#xff0c;所以是O(1) 稳定性 不稳定。 C代码&#xff08;大根堆&#xff09; cla…
阅读更多...
Jenkins持续集成之修改jenkins工作目录

Jenkins持续集成之修改jenkins工作目录

修改jenkins工作目录 一般不建议把工作目录放到默认的C盘&#xff0c;故可以更改到其他盘中 前置条件&#xff1a;先在其他盘中新建工作目录的文件&#xff1b;如下图 1、首先打开任务管理器&#xff0c;找到服务中的Jenkins进程 2、右击点击转到详细信息&#xff1b; 3、再右…
阅读更多...
洛谷P1706全排列问题题解（两种方法+详解）

洛谷P1706全排列问题题解（两种方法+详解）

问题引出 全排列问题 题目描述 按照字典序输出自然数 1 1 1 到 n n n 所有不重复的排列&#xff0c;即 n n n 的全排列&#xff0c;要求所产生的任一数字序列中不允许出现重复的数字。 输入格式 一个整数 n n n。 输出格式 由 1 ∼ n 1 \sim n 1∼n 组成的所有不重…
阅读更多...
成为Jmeter大师：从入门到精通的环境搭建教程

成为Jmeter大师：从入门到精通的环境搭建教程

一、JMeter 介绍 Apache JMeter是100%纯JAVA桌面应用程序&#xff0c;被设计为用于测试客户端/服务端结构的软件(例如web应用程序)。它可以用来测试静态和动态资源的性能&#xff0c;例如&#xff1a;静态文件&#xff0c;Java Servlet,CGI Scripts,Java Object,数据库和FTP服…
阅读更多...
什么是 http 代理，为什么需要 http 代理？

什么是 http 代理，为什么需要 http 代理？

前言&#xff1a; 在我们进行软件测试工作的时候&#xff0c;会有很多地方需要去设置代理的。 比如&#xff1a;fiddler抓包&#xff0c;jmeter录制脚本等等。 甚至于&#xff0c;在某些公司&#xff0c;去访问某些内部网址的时候&#xff0c;都需要通过连接vpn才能成功访问。…
阅读更多...
Meaning of life :Do not wait too long and miss the opportunity

Meaning of life :Do not wait too long and miss the opportunity

生命的意义在于机会在眼前&#xff0c;我们就要去抓住他&#xff0c;错过了可能会遗憾终身&#xff0c;不要去等待机会的到来&#xff0c;很多时候真是应一句老话, "机不可失时不再来!" 。 Dont wait too long and miss the opportunity Each spring brings a blos…
阅读更多...
瑞吉外卖 - 修改菜品功能（18）

瑞吉外卖 - 修改菜品功能（18）

某马瑞吉外卖单体架构项目完整开发文档&#xff0c;基于 Spring Boot 2.7.11 JDK 11。预计 5 月 20 日前更新完成&#xff0c;有需要的胖友记得一键三连&#xff0c;关注主页 “瑞吉外卖” 专栏获取最新文章。 相关资料&#xff1a;https://pan.baidu.com/s/1rO1Vytcp67mcw-PD…
阅读更多...
Flink第四章:水位线和窗口

Flink第四章:水位线和窗口

系列文章目录 Flink第一章:环境搭建 Flink第二章:基本操作. Flink第三章:基本操作(二) Flink第四章:水位线和窗口 文章目录 系列文章目录前言一、水位线二、窗口二、实际案例1.自定义聚合函数2.全窗口函数3.水位线窗口4.统计用户点击数据5.处理迟到数据 总结 前言 这次博客记…
阅读更多...
ChatGPT、GPT4、AutoGPT 和 MemoryGPT：初学者指南

ChatGPT、GPT4、AutoGPT 和 MemoryGPT：初学者指南

人工智能 (AI) 不仅在改变行业&#xff0c;也在改变我们的日常生活。借助人工智能&#xff0c;我们可以改善我们的组织和生产力&#xff0c;让我们能够专注于真正重要的事情。在本文中&#xff0c;我们将探讨一些适用于日常生活的 AI 工具&#xff0c;以及它们如何帮助您保持井…
阅读更多...
【Spring框架】--02.容器IoC、原理(手写IoC)

【Spring框架】--02.容器IoC、原理(手写IoC)

文章目录 3.容器&#xff1a;IoC3.1 IoC容器3.1.1 控制反转&#xff08;IoC&#xff09;3.1.2 依赖注入3.1.3 IoC容器在Spring的实现 3.2 基于XML管理Bean3.2.1搭建子模块spring6-ioc-xml3.2.2 获取bean①方式一&#xff1a;根据id获取②方式二&#xff1a;根据类型获取③方式三…
阅读更多...
【Jmeter第三章】Jmeter给请求添加请求头

【Jmeter第三章】Jmeter给请求添加请求头

给请求加上请求头最常见的场景就是在请求头上添加token了&#xff0c;这里也拿添加token来举例 1、添加某个请求的请求头 1、选中HTTP请求&#xff0c;右键添加 2、添加请求头 2、添加公共的请求头信息 其实步骤和上面是一样的&#xff0c;只不过是选择&#xff1a;线程组…
阅读更多...
极客的git常用命令手册

极客的git常用命令手册

极客的git常用命令手册 1.1 权限配置篇1.1.1 创建ssh key1.1.2 本地存在多个密钥时&#xff0c;如何根据目标平台自动选择用于认证的密钥&#xff1f; 1.2 基础信息配置篇1.2.1 配置用户名1.2.2 配置用户邮箱1.2.3 设置文件名大小写区分1.2.4 设置命令行显示颜色1.2.5 检查git全…
阅读更多...
MySQL高级_第11章_数据库的设计规范

MySQL高级_第11章_数据库的设计规范

MySQL高级_第11章_数据库的设计规范 1. 为什么需要数据库设计 2. 范 式 2.1 范式简介 在关系型数据库中&#xff0c;关于数据表设计的基本原则、规则就称为范式。 可以理解为&#xff0c;一张数据表的设计结构需要满足的某种设计标准的 级别 。要想设计一个结构合理的关…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023