SLAM小题目

2024-06-10 05:36
文章标签 slam 小题目

本文主要是介绍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;
}

这篇关于SLAM小题目的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1047337。

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
激光SLAM如何动态管理关键帧和地图

激光SLAM如何动态管理关键帧和地图

0. 简介 个人在想在长期执行的SLAM程序时，当场景发生替换时，激光SLAM如何有效的更新或者替换地图是非常关键的。在看了很多Life-Long的文章后，个人觉得可以按照以下思路去做。这里可以给大家分享一下 <br/> 1. 初始化保存关键帧 首先对应的应该是初始化设置，初始化设置当中会保存关键帧数据，这里的对应的关键帧点云数据会被存放在history_kf_lidar当中，这个数据是和
阅读更多...
用python fastapi写一个http接口，使ros2机器人开始slam toolbox建图

用python fastapi写一个http接口，使ros2机器人开始slam toolbox建图

如果你想使用Python的FastAPI框架编写一个HTTP接口，以便通过接口启动ROS 2机器人的SLAM Toolbox建图，可以按照以下方式进行： 首先，确保你已经安装了fastapi和uvicorn库。你可以使用以下命令进行安装： pip install fastapi uvicorn 接下来，创建一个Python文件（例如app.py），并将以下代码添加到文件中： import
阅读更多...
GS-SLAM论文阅读笔记--GSFusion

GS-SLAM论文阅读笔记--GSFusion

介绍 GS-SLAM是最近比较新的方向，由于传统SLAM的研究变得很少，拥抱与新的技术结合的方法也许是个好主意。之前总结了大部分GS-SLAM的文章。但是这个方向在不断发展，而发展初期的很多论文值得参考。所以用博客记录一下比较新的论文阅读笔记。GSFusion 这篇文章是TUM发表的，TUM在SLAM领域是非常牛的，所以需要仔细阅读一下这篇文章，肯定会有所收获！ 文章目录 介绍1.
阅读更多...
SLAM相关数据集调研

SLAM相关数据集调研

SLAM相关数据集调研 主要分成4种：关于自动驾驶的数据集，包含Depth的SLAM及三维重建数据集，不含Depth的数据集，包含语义的数据集。 自动驾驶： KITTI数据集：http://www.cvlibs.net/datasets/kitti/index.php （RGB+Lidar+GPS+IMU） KITTI数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是目前国际上最
阅读更多...
SLAM ORB-SLAM2（29）PnP估计姿态

SLAM ORB-SLAM2（29）PnP估计姿态

SLAM ORB-SLAM2（29）PnP估计姿态 1. PnP问题2. EPnP算法2.1. 计算4对控制点的世界坐标2.2. 计算齐次质心坐标2.3. 计算4对控制点的相机坐标2.3.1. 构造M矩阵2.3.2. 计算 M T M M^TM MTM的0特征值对应的特征向量2.3.3. 计算零空间的秩2.3.4. 计算线性组合的系数 2.4. 选择最小重投影误差 3. 标题
阅读更多...
SLAM_极线搜索最佳匹配特征点_NCC

SLAM_极线搜索最佳匹配特征点_NCC

目录 1. 需求 2. 代码实现 思路 代码 极线搜索 NCC的实现 双线性插值 1. 需求 输入: 参考图像ref, 当前图像curr, 当前图像相对于参考图像的位姿T_C_R,  参考图像的深度图depth和深度方差图depth_cov, 输入参考图像ref上的一个像素点坐标pt_ref, 求解: 在当前图像上的最佳匹配点pt_curr 2. 代码实现 思路
阅读更多...
视觉SLAM补充习题（来源于B站博主）

视觉SLAM补充习题（来源于B站博主）

题目来源于up主全日制学生混的SLAM课程，代码链接如下： GitHub - cckaixin/Practical_Homework_for_slambook14: This repository is used to store SLAM 14 training exercises 如果打不开github请参考下面两篇博文，亲测有效！！ 解决国内 github.com 打不开的最最最准确方
阅读更多...
DSO slam ros 模式

DSO slam ros 模式

dso 直接法纯视觉定位,作者采用读取包的方式,数据使用存在不方便. 源码:https://github.com/JakobEngel/dso.git ros需要链接库版:https://github.com/JakobEngel/dso_ros.git 直接运行版:GitHub - jankinbyy/dso_ros_run 在config下修改相机内参. 结果:地图点密度较高,尺度存
阅读更多...
Gazebo Harmonic gz-harmonic 和 ROS2 Jazzy 思考题 建图和导航 SLAM Navigation

Gazebo Harmonic gz-harmonic 和 ROS2 Jazzy 思考题 建图和导航 SLAM Navigation

仿真 效果还挺好的。  SLAM建图 导航 …… 提示 这篇文档详细介绍了如何在ROS 2环境中使用SLAM（Simultaneous Localization and Mapping，即同时定位与地图构建）和Nav2（Navigation 2，ROS 2的导航框架）来让机器人一边构建环境地图一边进行导航。以下是对该文档的详细总结： 概述 文档主要面向ROS 2用户
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2