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 OpenCV3Ptr<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 Freiburg2Mat 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);// 内参矩阵KMatrix3f 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.mzph.cn/pingmian/25849.shtml

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

相关文章

监听DB配置变更之go-broadcast简单实现

监听DB配置变更之go-broadcast简单实现

文章目录 1. 前言2. 分析3. 实现4. 问题5. 小结6. 参考 1. 前言 之前遇到一个需求&#xff0c;因为配置的查找是基于db的&#xff0c;而db的更改却无法实时通知到具体利用到这条数据的使用方&#xff0c;为了实现db数据变动时&#xff0c;能够尽快让使用方知道这条数据发生了变…
阅读更多...
3、线性代数

3、线性代数

1、矩阵转置 A[i,j]A[j,i] 2、对称矩阵 &#xff1a;A转置A [0,2,3] [2 1 5] [3,5,1] 3、三维矩阵 求和 axis0 两个矩阵相加 axis1 两个向量相加 &#xff0c;axis2 向量内部相加 keepdimsTrue 求和后维度保持不变 4、cumsum累加求和 5、torch.mm() 或 torch.bmm() 【矩…
阅读更多...
支持YUV和RGB格式两路视频同时播放

支持YUV和RGB格式两路视频同时播放

1.头文件&#xff1a; sdlqtrgb.h #pragma once #include <QtWidgets/QWidget> #include "ui_sdlqtrgb.h" #include <thread> class SdlQtRGB : public QWidget {Q_OBJECTpublic:SdlQtRGB(QWidget* parent Q_NULLPTR);~SdlQtRGB(){is_exit_ true;//等…
阅读更多...
十大排序

十大排序

本文将以「 通俗易懂」的方式来描述排序的基本实现。 &#x1f9d1;‍&#x1f4bb;阅读本文前&#xff0c;需要一点点编程基础和一点点数据结构知识 本文的所有代码以cpp实现 文章目录 排序的定义 插入排序 ⭐ &#x1f9d0;算法描述 &#x1f496;具体实现 &#x1f…
阅读更多...
LabVIEW硬件与仿真双模式设计液压系统

LabVIEW硬件与仿真双模式设计液压系统

在实际项目中&#xff0c;结合LabVIEW与液压运动控制器&#xff0c;通过设计两种运行方式&#xff1a;硬件运行和仿真运行&#xff0c;使得系统既能进行实际操作又能进行仿真测试。此设计不仅方便了开发调试&#xff0c;也为教学培训和展示提供了极大的便利。本文将从项目背景、…
阅读更多...
【机器学习】基于卷积LSTM的视频预测

【机器学习】基于卷积LSTM的视频预测

1. 引言 1.1 LSTM是什么 LSTM&#xff08;Long Short-Term Memory&#xff09;是一种特殊的循环神经网络&#xff08;RNN&#xff09;变体&#xff0c;旨在解决传统RNN在处理长序列数据时遇到的梯度消失和梯度爆炸问题。LSTM通过引入门控机制和细胞状态的概念&#xff0c;使得…
阅读更多...
QPS，平均时延和并发数

QPS，平均时延和并发数

我们当前有两个服务A和B&#xff0c;想要知道哪个服务的性能更好&#xff0c;该用什么指标来衡量呢&#xff1f; 1. 单次请求时延 一种最简单的方法就是使用同一请求体同时请求两个服务&#xff0c;性能越好的服务时延越短&#xff0c;即 R T 返回结果的时刻 − 发送请求的…
阅读更多...
【Python教程】4-字符串、列表、字典、元组与集合操作

【Python教程】4-字符串、列表、字典、元组与集合操作

在整理自己的笔记的时候发现了当年学习python时候整理的笔记&#xff0c;稍微整理一下&#xff0c;分享出来&#xff0c;方便记录和查看吧。个人觉得如果想简单了解一名语言或者技术&#xff0c;最简单的方式就是通过菜鸟教程去学习一下。今后会从python开始重新更新&#xff0…
阅读更多...
AI大模型探索之路-实战篇16：优化决策流程：Agent智能数据分析平台中Planning功能实践

AI大模型探索之路-实战篇16：优化决策流程：Agent智能数据分析平台中Planning功能实践

系列篇章&#x1f4a5; AI大模型探索之路-实战篇4&#xff1a;深入DB-GPT数据应用开发框架调研 AI大模型探索之路-实战篇5&#xff1a;探索Open Interpreter开放代码解释器调研 AI大模型探索之路-实战篇6&#xff1a;掌握Function Calling的详细流程 AI大模型探索之路-实战篇7…
阅读更多...
现代x86汇编-环境安装

现代x86汇编-环境安装

今天端午节&#xff0c;独自在家&#xff0c;翻阅了张银奎老师编写的《现代x86汇编语言程序设计》一书&#xff0c;前言部分说明书中示例代码都是用微软visual C工具编写并使用微软宏汇编&#xff08;著名的MASM&#xff09;编译的&#xff0c;好久没有用微软vc了&#xff0c;假…
阅读更多...
详解FedProx：FedAvg的改进版 Federated optimization in heterogeneous networks

详解FedProx：FedAvg的改进版 Federated optimization in heterogeneous networks

FedProx&#xff1a;2020 FedAvg的改进 论文&#xff1a;《Federated Optimization in Heterogeneous Networks》 引用量&#xff1a;4445 源码地址&#xff1a; 官方实现&#xff08;tensorflow&#xff09;https://github.com/litian96/FedProx 几个pytorch实现&#xff1a;…
阅读更多...
基于STM32的595级联的Proteus仿真

基于STM32的595级联的Proteus仿真

文章目录 一、595级联1.题目要求2.思路3.仿真图3.1 未仿真时3.2 模式A3.2 模式B3.3 故障模式 二、总结 一、595级联 1.题目要求 STM32单片机&#xff0c;以及三个LED灯对应红黄绿灯&#xff0c;IIC的OLED显示屏&#xff0c;温湿度传感器DHT11&#xff0c;两个独立按键和两个5…
阅读更多...
MySQL时间和日期类型详解（零基础入门篇）

MySQL时间和日期类型详解（零基础入门篇）

目录 1. DATE 2. DATETIME 3. TIMESTAMP 4. TIME 5. YEAR 6. 日期和时间的使用示例 以下SQL语句的测试可以使用命令行&#xff0c;或是使用SQL工具比如MySQL Workbench或SQLynx等。 在 MySQL 中&#xff0c;时间和日期数据类型用于存储与时间相关的数据&#xff0c;如何…
阅读更多...
重温共射放大电路

重温共射放大电路

1、放大概念 小功率信号变成一个大功率信号&#xff0c;需要一个核心器件做这件事&#xff0c;核心器件的能量由电源提供&#xff0c;通过核心器件用小功率的信号去控制大电源&#xff0c;来实现能量的转换和控制&#xff0c;前提是不能失真&#xff0c;可以用一系列正弦波进行…
阅读更多...
Running Gradle task ‘assembleDebug‘ Flutter项目

Running Gradle task ‘assembleDebug‘ Flutter项目

基于Android方面运行Flutter项目一直卡在 Launching lib\main.dart on Android SDK built for x86 in debug mode… Running Gradle task ‘assembleDebug’… 基础原因&#xff1a; 默认存放Gradle插件包的Maven仓库是国外(需VPN) 我的原因&#xff1a; 缺少JDK和缺少Androi…
阅读更多...
【Oracle】Oracle导入导出dmp文件

【Oracle】Oracle导入导出dmp文件

文章目录 前言一、什么是dmp&#xff1f;二、imp/impdp、exp/expdp对比及示例1.区别2.imp/impdp对比及示例a. impb. impbp 3.exp/expdp对比及示例a. expb.expdp 3.其他事项 三、执行导入导出前置条件1.创建角色并授权2.创建目录映射 前言 在工作中&#xff0c;经常会遇到需要备…
阅读更多...
React中的 Scheduler

React中的 Scheduler

为什么需要调度 在 React 中&#xff0c;组件最终体现为 Fiber&#xff0c;并形成 FiberTree&#xff0c;Fiber 的目的是提高渲染性能&#xff0c;将原先的 React 渲染任务拆分为多个小的微任务&#xff0c;这样做的目的是可以灵活的让出主线程&#xff0c;可以随时打断渲染&a…
阅读更多...
定个小目标之刷LeetCode热题（10）

定个小目标之刷LeetCode热题（10）

这道题属于一道中等题&#xff0c;看来又得背题了&#xff0c;直接看题解吧&#xff0c;有两种解法 第一种动态规划法 状态&#xff1a;dp[i][j] 表示字符串s在[i,j]区间的子串是否是一个回文串 状态转移方程&#xff1a;当s[i] s[j] && (j - i < 2 || dp[i 1]…
阅读更多...
讨论C++类与对象

讨论C++类与对象

讨论C类与对象 C语言结构体和C类的对比类的实例化类对象的大小猜想一猜想二针对上述猜想的实践 this指针不同对象调用成员函数 类的6个默认成员函数构造函数析构函数拷贝构造函数浅拷贝和深拷贝 赋值运算符重载 初始化列表初始化顺序 C语言结构体和C类的对比 在C语言中&#x…
阅读更多...
对猫毛过敏？怎么有效的缓解过敏症状，宠物空气净化器有用吗？

对猫毛过敏？怎么有效的缓解过敏症状，宠物空气净化器有用吗？

猫过敏是一种常见的过敏反应&#xff0c;由猫的皮屑、唾液或尿液中的蛋白质引起。这些蛋白质被称为过敏原&#xff0c;它们可以通过空气传播&#xff0c;被人体吸入后&#xff0c;会触发免疫系统的过度反应。猫过敏是宠物过敏中最常见的类型之一&#xff0c;对许多人来说&#…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023