SLAM小题目

1、最小二乘题目：

        假设有三个WIFI热点，位置分别在(x1,y1), (x2,y2), (x3,y3), 移动端测量到每一个热点的距离L1,L2和L3，要求解移动端的位置. 

#include <iostream>
#include <vector>
#include <cmath> 

class Point {
public:
    double x, y;
};

Point test(const std::vector<Point>& location, const std::vector<double>& distances) {

    double x1 = location[0].x, y1 = location[0].y, L1 = distances[0];
    double x2 = location[1].x, y2 = location[1].y, L2 = distances[1];
    double x3 = location[2].x, y3 = location[2].y, L3 = distances[2];

    double a1 = 2 * (x2 - x1);
    double b1 = 2 * (y2 - y1);
    double c1 = L1 * L1 - L2 * L2 + x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1;

    double a2 = 2 * (x3 - x1);
    double b2 = 2 * (y3 - y1);
    double c2 = L1 * L1 - L3 * L3 + x3 * x3 - x1 * x1 + y3 * y3 - y1 * y1;

    Point result;
    result.x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
    result.y = (a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1);

    return result;
}

int main() {
    std::vector<Point> location = {{0, 0}, {1, 0}, {0, 1}};
    double sqrt_2 = std::sqrt(2.0);
    std::vector<double> distances = {sqrt_2, 1.0, 1.0};
    Point result = test(location, distances);
    std::cout << "移动端的位置是 (" << result.x << ", " << result.y << ")" << std::endl;
    return 0;
}

2、图像题目

已知相机内参：520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1。求解两帧图像之间的运动。

    a.部署OpenCV等环境，使用熟悉的特征点匹配后计算。

    b.运动求解过程需要亲自编写程序，不允许调用第三方库。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(vo1)

set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

find_package(OpenCV 3.4.15 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)

include_directories(
        ${OpenCV_INCLUDE_DIRS}
       # ${G2O_INCLUDE_DIRS}
       # ${Sophus_INCLUDE_DIRS}
        "/usr/include/eigen3/"
)

# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2 
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})

主函数

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <vector>
#include <random>
#include <ctime>
#include <Eigen/Dense>
using namespace std;
using namespace cv;
using namespace Eigen;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void pose_estimation_2d2d(
  std::vector<KeyPoint> keypoints_1,
  std::vector<KeyPoint> keypoints_2,
  std::vector<DMatch> matches,
  Mat &R, Mat &t);

const int MAX_ITER = 100000;  // 最大迭代次数
const float THRESHOLD = 0.001;  // 内点距离阈值


float calculateError(const Matrix3f& F, const Point2f& pt1, const Point2f& pt2) {
    Vector3f p1(pt1.x, pt1.y, 1.0);
    Vector3f p2(pt2.x, pt2.y, 1.0);
    Vector3f line = F * p1;
    return fabs(p2.dot(line)) / sqrt(line[0] * line[0] + line[1] * line[1]);
}

Matrix3f ransacFundamentalMat(const vector<Point2f>& points1, const vector<Point2f>& points2) {
    int bestInliersCount = 0;
    Matrix3f bestF;
    std::default_random_engine rng(time(0));
    std::uniform_int_distribution<int> uniDist(0, points1.size() - 1);

    for (int iter = 0; iter < MAX_ITER; ++iter) {
        vector<Point2f> samplePoints1, samplePoints2;
        for (int i = 0; i < 8; ++i) {
            int idx = uniDist(rng);
            samplePoints1.push_back(points1[idx]);
            samplePoints2.push_back(points2[idx]);
        }

        int N = samplePoints1.size();
        MatrixXf A(N, 9);

        for (int i = 0; i < N; ++i) {
            float x1 = samplePoints1[i].x;
            float y1 = samplePoints1[i].y;
            float x2 = samplePoints2[i].x;
            float y2 = samplePoints2[i].y;
            A.row(i) << x2 * x1, x2 * y1, x2, y2 * x1, y2 * y1, y2, x1, y1, 1;
        }

        JacobiSVD<MatrixXf> svd(A, ComputeFullV);
        VectorXf f = svd.matrixV().col(8);

        Matrix3f F;
        F << f(0), f(1), f(2),
                f(3), f(4), f(5),
                f(6), f(7), f(8);

        JacobiSVD<Matrix3f> svdF(F, ComputeFullU | ComputeFullV);
        Vector3f singularValues = svdF.singularValues();
        singularValues[2] = 0;
        F = svdF.matrixU() * singularValues.asDiagonal() * svdF.matrixV().transpose();

        int inliersCount = 0;
        for (int i = 0; i < points1.size(); ++i) {
            if (calculateError(F, points1[i], points2[i]) < THRESHOLD) {
                ++inliersCount;
            }
        }

        if (inliersCount > bestInliersCount) {
            bestInliersCount = inliersCount;
            cout<<bestInliersCount<<endl;
            bestF = F;
        }
    }

    return bestF;
}
void recoverPose_my(const Matrix3f& essential_matrix, const vector<Point2f>& points1, const vector<Point2f>& points2, Matrix3f& R, Vector3f& t, double focal_length, const Point2d& principal_point) {
    // SVD分解本质矩阵
    JacobiSVD<Matrix3f> svd(essential_matrix, ComputeFullU | ComputeFullV);
    Matrix3f U = svd.matrixU();
    Matrix3f Vt = svd.matrixV().transpose();

    // 构造W矩阵和Z矩阵
    Matrix3f W;
    W << 0, -1, 0,
            1, 0, 0,
            0, 0, 1;

    Matrix3f Z;
    Z << 0, 1, 0,
            -1, 0, 0,
            0, 0, 0;

    // 可能的两种旋转矩阵
    Matrix3f R1 = U * W * Vt;
    if (R1.determinant() < 0) {
        R1 = -R1;
    }
    Matrix3f R2 = U * W.transpose() * Vt;
    if (R2.determinant() < 0) {
        R2 = -R2;
    }

    // 平移向量
    Vector3f t1 = U.col(2);
    Vector3f t2 = -U.col(2);

    // 对两个可能的解进行验证，选择最合适的那个
    int valid_count1 = 0;
    int valid_count2 = 0;

    for (int i = 0; i < points1.size(); ++i) {
        Vector3f p1(points1[i].x, points1[i].y, 1.0);
        Vector3f p2(points2[i].x, points2[i].y, 1.0);

        p1[0] = (p1[0] - principal_point.x) / focal_length;
        p1[1] = (p1[1] - principal_point.y) / focal_length;
        p2[0] = (p2[0] - principal_point.x) / focal_length;
        p2[1] = (p2[1] - principal_point.y) / focal_length;

        // 计算重投影误差来判断是否在相机前方
        Vector3f p1_proj = R1 * p2 + t1;
        if (p1_proj[2] > 0) {
            ++valid_count1;
        }

        p1_proj = R2 * p2 + t2;
        if (p1_proj[2] > 0) {
            ++valid_count2;
        }
    }

    if (valid_count1 > valid_count2) {
        R = R1;
        t = t1;
    } else {
        R = R2;
        t = t2;
    }
}

int main() {

  //-- 读取图像
  Mat img_1 = imread("../1.png", CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread("../2.png", CV_LOAD_IMAGE_COLOR);
    if (img_1.empty() || img_2.empty()) {
        cerr << "无法加载图片" << endl;
        return -1;
    }

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

  return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
    Mat img_matches;
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_matches);

    // 显示匹配结果
    imshow("按任意键继续！", img_matches);
    waitKey();
}


void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);  //相机光心
  double focal_length = 521;      //相机焦距
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);

  // 内参矩阵K
  Matrix3f k;
  k << focal_length, 0, principal_point.x,
   0, focal_length, principal_point.y,
   0, 0, 1;

  // 计算本质矩阵
  Matrix3f fundamental_matrix_ransac;
  fundamental_matrix_ransac =  ransacFundamentalMat(points1, points2);
  Matrix3f E = k.transpose() * fundamental_matrix_ransac * k;
  JacobiSVD<Matrix3f> svdE(E, ComputeFullU | ComputeFullV);
  Vector3f singularValues = svdE.singularValues();
  singularValues[2] = 0;
  E = svdE.matrixU() * singularValues.asDiagonal() * svdE.matrixV().transpose();
  //-- 从本质矩阵中恢复旋转和平移信息.
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  Matrix3f R1;
  Vector3f t1;
  recoverPose_my(  E , points1, points2, R1, t1, focal_length, principal_point);
  cout << "调用opencv计算的结果： " << endl;
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;
  cout <<  "使用ransac自行计算的结果： " << endl;
  cout << "R is " << endl << R1 << endl;
  cout << "t is " << endl << t1 << endl;
}

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

ssm601基于ssm框架的校园闲置物品交易平台+jsp【已测试】

ssm601基于ssm框架的校园闲置物品交易平台+jsp【已测试】

前言&#xff1a;&#x1f469;‍&#x1f4bb; 计算机行业的同仁们&#xff0c;大家好&#xff01;作为专注于Java领域多年的开发者&#xff0c;我非常理解实践案例的重要性。以下是一些我认为有助于提升你们技能的资源&#xff1a; &#x1f469;‍&#x1f4bb; SpringBoot…
阅读更多...
抓住时机的核心：坚持学习准备着

抓住时机的核心：坚持学习准备着

在这个快节奏的时代&#xff0c;时间对于每个人来说都是宝贵的。能否在合适的时间做正确的事情&#xff0c;往往决定了我们成功的概率。但同时&#xff0c;我们也要认识到&#xff0c;逆风翻盘虽少&#xff0c;却并非不可能。在这个过程中&#xff0c;投资自己&#xff0c;投资…
阅读更多...
28-LINUX--I/O复用-epoll

28-LINUX--I/O复用-epoll

一.epoll概述 epoll 是 Linux 特有的 I/O 复用函数。它在实现和使用上与 select、poll 有很大差异。首 先&#xff0c;epoll 使用一组函数来完成任务&#xff0c;而不是单个函数。其次&#xff0c;epoll 把用户关心的文件描述 符上的事件放在内核里的一个事件表中。从而无需像…
阅读更多...
计算机网络--应用层

计算机网络--应用层

计算机网络–计算机网络概念 计算机网络–物理层 计算机网络–数据链路层 计算机网络–网络层 计算机网络–传输层 计算机网络–应用层 1. 概述 因为不同的网络应用之间需要有一个确定的通信规则。 1.1 两种常用的网络应用模型 1.1.1 客户/服务器模型&#xff08;Client/Se…
阅读更多...
[office] excel工作表数据分级显示 #其他#笔记

[office] excel工作表数据分级显示 #其他#笔记

excel工作表数据分级显示 如下图1所示的工作表数据&#xff0c;我们按东区、西区、南区、北区来建立分级显示。 图1 这里先利用“创建组”命令建立分级显示。选取单元格区域A3:E5&#xff0c;单击功能区“数据”选项卡“分级显示”组中的“创建组——创建组…”命令&#xff…
阅读更多...
基于小波多分辨分析的一维时间序列信号趋势检测与去除(MATLAB R2018a)

基于小波多分辨分析的一维时间序列信号趋势检测与去除(MATLAB R2018a)

小波最开始是数学上提出的概念&#xff0c;并且在纯数学的王国里存在了一个世纪之久。最开始是为了弥补傅里叶分析的缺陷&#xff0c;即傅里叶级数发散的问题&#xff0c;并寻找出能够代替傅里叶分析的方法。从最早的一些艰难的探索开始直到慢慢发展成为一套完整系统的小波分析…
阅读更多...
Git配置 安装及使用

Git配置 安装及使用

团队开发的神 找工作必备 环境变量 配置好环境后 打开终端环境 winr cmd 我习惯在桌面打开&#xff0c;然后进入相应的文件夹 &#xff08;文件夹结构&#xff09; &#xff08;个人感觉能用cmd不用git&#xff0c;cmd更好用一些&#xff09; 进入对应的文件夹 填写自己对…
阅读更多...
Web3设计风格和APP设计风格

Web3设计风格和APP设计风格

Web3设计风格和传统APP设计风格在视觉和交互设计上有一些显著的区别。这些差异主要源于Web3技术和理念的独特性&#xff0c;以及它们在用户体验和界面设计中的具体应用。以下是Web3设计风格与传统APP设计风格的主要区别。北京木奇移动技术有限公司&#xff0c;专业的软件外包开…
阅读更多...
Elasticsearch最佳生产实践整理，推荐收藏

Elasticsearch最佳生产实践整理，推荐收藏

前言 Elasticsearch是一个底层基于Lucene的全文搜索和分析引擎&#xff0c;支持近乎实时地存储、搜索和分析大量数据的能力&#xff0c;最常用于网站搜索、日志搜索、数据分析等场景。 本文主要针对日常工作中Elasticsearch使用的一些基础概念、使用规范、注意事项、常见优化…
阅读更多...
GPU风扇不旋转：为什么会发生这种情况以及如何修复

GPU风扇不旋转：为什么会发生这种情况以及如何修复

GPU在处理数百万像素时往往会发热，因此冷却风扇静音可能会令人担忧，这是可以理解的！如果你注意到你的GPU风扇没有旋转，下面是如何评估是否存在真正的问题，以及如何解决问题。 风扇停止旋转可能是一个功能，而不是一个Bug 如果GPU没有用于密集任务或没有达到高温，则可以…
阅读更多...
1V1音视频实时互动直播系统

1V1音视频实时互动直播系统

李超老师的项目 先肯定分为两个两个端&#xff0c;一个是服务器端一个是客户端。客户端用于UI界面的显示&#xff0c;服务器端用于处理客户端发来的消息。 我们先搭建stun和turn服务器 首先介绍一下什么是stun协议&#xff0c; 它是用来干什么的&#xff1f; stun协议存在…
阅读更多...
构建第一个ArkTS应用之@卡片使用动效能力

构建第一个ArkTS应用之@卡片使用动效能力

ArkTS卡片开放了使用动画效果的能力&#xff0c;支持显式动画、属性动画、组件内转场能力。需要注意的是&#xff0c;ArkTS卡片使用动画效果时具有以下限制&#xff1a; 表1 动效参数限制 名称 参数说明 限制描述 duration 动画播放时长 限制最长的动效播放时长为1秒&…
阅读更多...
把文件从一台linux机器上传到另一台linux机器上

把文件从一台linux机器上传到另一台linux机器上

文章目录 1&#xff0c;第一种情况1.1 先测试2台机器是否可以互相通信1.2 对整个文件夹里面的所有内容进行传输的命令1.3 检查结果 2&#xff0c;第二种情况2.1&#xff0c;单个文件传输的命令 1&#xff0c;第一种情况 我这里有2台linux机器&#xff0c; 机器A&#xff1a;19…
阅读更多...
NDIS Filter开发-PNP响应和安装

NDIS Filter开发-PNP响应和安装

NDIS filter驱动可能是最容易生成的驱动之一&#xff0c;如果你安装了VS 2015 WDK之后&#xff0c;你可以直接生成一个能运行的Filter驱动&#xff0c;它一般是ndislwf。 和大部分硬件不同&#xff0c;NDIS Filter驱动介于软件和硬件抽象层之上&#xff0c;它和硬件相关&…
阅读更多...
AI2THOR 2.1.0使用教程

AI2THOR 2.1.0使用教程

一、安装和入门 1.1 AI2-THOR使用要求 操作系统&#xff1a; Mac OS X 10.9&#xff0c; Ubuntu 14.04显卡&#xff1a;DX9&#xff08;着色器型号 3.0&#xff09;或 DX11&#xff0c;功能级别为 9.3。CPU&#xff1a;支持 SSE2 指令集。Python 2.7 或 Python 3.5Linux 用户…
阅读更多...
FuTalk设计周刊-Vol.040

FuTalk设计周刊-Vol.040

&#x1f525;AI漫谈 热点捕手 1、零代码定制游戏NPC&#xff0c;百川智能发布角色大模型 百川智能此次推出了“角色创建平台搜索增强知识库”的定制化解决方案。通过这一方案&#xff0c;游戏厂商无需编写任何代码&#xff0c;只需通过简单的文字描述&#xff0c;便可以快速…
阅读更多...
28-unittest批量执行（discover）

28-unittest批量执行（discover）

unittest框架提供了创建测试用例、测试套件以及批量执行的解决方案。 利用单元测试框架创建测试类&#xff0c;可以把每个测试方法看成是一个最小的单元&#xff0c; 由测试容器组装打包起来&#xff0c;然后可以统一执行&#xff0c;最后输出测试报告。 一、UnitTest核心要素…
阅读更多...
OpenCV如何判断一张图片是否有过高的明暗变化

OpenCV如何判断一张图片是否有过高的明暗变化

操作系统&#xff1a;ubuntu22.04OpenCV版本&#xff1a;OpenCV4.9IDE:Visual Studio Code编程语言&#xff1a;C11 前言 判断一张图片是否有过高的明暗变化&#xff0c;可以通过分析图像的亮度分布一致性来实现。一种常见的做法是计算图像的亮度标准差&#xff08;Standard …
阅读更多...
JAVA技术设计模式

JAVA技术设计模式

设计模式结构图 设计原则 职责单一原则接口隔离原则 一个类对另一个类的依赖应该建立在最小的接口上 依赖倒置面向接口编程，参数或变量，依赖注入，使用父类 开闭原则 对扩展开放（对提供方），对修改关闭（对使用方） 用抽象构建框架，用实现扩展细节 里氏替换原则…
阅读更多...
计算机网络--物理层

计算机网络--物理层

计算机网络--计算机网络概念 计算机网络--物理层 计算机网络--数据链路层 计算机网络--网络层 计算机网络--传输层 计算机网络--应用层 1. 基本概念 物理层的概念&#xff1a;物理层解决如何在在连接各种计算机的传输媒体上传输数据比特流&#xff0c;而不是指具体的传输…
阅读更多...
最新文章

Copyright @ 2022~2023