自学SLAM（8）《第四讲：相机模型与非线性优化》作业

前言

在这里插入图片描述

小编研究生的研究方向是视觉SLAM，目前在自学，本篇文章为初学高翔老师课的第四次作业。

文章目录

  • 前言
  • 1.图像去畸变
  • 2.双目视差的使用
  • 3.矩阵微分
  • 4.高斯牛顿法的曲线拟合实验

1.图像去畸变

现实⽣活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使⽤⼴⾓和鱼眼镜头时，由于畸变的原因，直线在图像⾥看起来是扭曲的。本次作业，你将尝试如何对⼀张图像去畸变，得到畸变前的图像。
在这里插入图片描述
在这里插入图片描述

对于畸变，用两张鲜明的照片来展示：
在这里插入图片描述
在这里插入图片描述
undistort_image.cpp：

//
// Created by ljh on 2023/11/5.
//

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "/home/lih/video4_homework/homework1/test.png"; // 请确保路径正确

int main(int argc, char **argv) {

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {

            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
            // start your code here
            // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
double x = (u-cx)/fx, y = (v-cy)/fy; 

// 计算图像点坐标到光心的距离；
double r = sqrt(x*x+y*y);

// 计算投影点畸变后的点
double x_distorted = x*(1+k1*r+k2*r*r)+2*p1*x*y+p2*(r+2*x*x); 
double y_distorted = y*(1+k1*r+k2*r*r)+2*p2*x*y+p1*(r+2*y*y); 

// 把畸变后的点投影回去
u_distorted = x_distorted*fx+cx;
v_distorted = y_distorted*fy+cy;
            // end your code here

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }

    // 画图去畸变后图像
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

string image_file = “/home/lih/video4_homework/homework1/test.png”; // 填写你自己的图片路径

CMakeLists.txt：

cmake_minimum_required(VERSION 2.8)

PROJECT(undistort_image)

IF(NOT CMAKE_BUILD_TYPE) #（可选）如果没有指定cmake编译模式，就选择Relealse模式，必须写成三行
    SET(CMAKE_BUILD_TYPE Release)
ENDIF()

MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) #终端打印cmake编译模式的信息

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ") #添加c标准支持库
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3 -march=native") #添加c++标准支持库

# Check C++11 or C++0x support #检查c++11或c++0x标准支持库
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
    add_definitions(-DCOMPILEDWITHC11)
    message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
    add_definitions(-DCOMPILEDWITHC0X)
    message(STATUS "Using flag -std=c++0x.")
else()
    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

find_package(OpenCV 3.0 QUIET) #find_package(<Name>)命令首先会在模块路径中寻找 Find<name>.cmake
if(NOT OpenCV_FOUND)
    find_package(OpenCV 2.4.3 QUIET)
    if(NOT OpenCV_FOUND)
        message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
    endif()
endif()

include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(image undistort_image.cpp)

#链接OpenCV库
target_link_libraries(image ${OpenCV_LIBS})

然后
mkdir build
cd build
cmake …
make
./image
在这里插入图片描述

2.双目视差的使用

双⽬相机的⼀⼤好处是可以通过左右⽬的视差来恢复深度。课程中我们介绍了由视差计算深度的过程。本题，你需要根据视差计算深度，进⽽⽣成点云数据。本题的数据来⾃Kitti 数据集 [2]。 Kitti中的相机部分使⽤了⼀个双⽬模型。双⽬采集到左图和右图，然后我们可以通过左右视图恢复出深度。经典双⽬恢复深度的算法有 BM(Block Matching), SGBM(Semi-Global Matching)[3, 4]等，但本题不探讨⽴体视觉内容（那是⼀个⼤问题）。我们假设双⽬计算的视差已经给定，请你根据双⽬模型，画出图像对应的点云，并显⽰到 Pangolin 中。题给定的左右图见 code/left.png 和 code/right.png，视差图亦给定，见code/right.png。双⽬的参数如下：
fx= 718.856; fy = 718.856; cx =607.1928; cy = 185.2157
且双⽬左右间距（即基线）为：
d = 0.573 m
请根据以上参数，计算相机数据对应的点云，并显⽰到 Pangolin 中。程序请code/disparity.cpp ⽂件。
在这里插入图片描述

disparity.cpp：


           // start your code here
            // 根据双目模型计算 point 的位置
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;
             // end your code here

double x和double y的计算方式和上一题一样,depth就算如下：
在这里插入图片描述
计算出depth后，那么point模仿课上五对图片那个实践仿写即可。只不过实践中的d(视差)没有给出，而此题中视差d已给，所以公式写出来略有不同。
CMakeLists.txt:

cmake_minimum_required( VERSION 2.8 )
project(stereoVision)
set( CMAKE_CXX_FLAGS "-std=c++11 -O3")
 
include_directories("/usr/include/eigen3")
find_package(Pangolin REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )
 
find_package(OpenCV 3.0 QUIET) #find_package(<Name>)命令首先会在模块路径中寻找 Find<name>.cmake
if(NOT OpenCV_FOUND)
    find_package(OpenCV 2.4.3 QUIET)
    if(NOT OpenCV_FOUND)
        message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
    endif()
endif()

include_directories(${OpenCV_INCLUDE_DIRS})
 
add_executable(disparity disparity.cpp)
target_link_libraries(disparity ${OpenCV_LIBRARIES})
target_link_libraries(disparity ${Pangolin_LIBRARIES})

然后就是编译五部曲：
mkdir build
cd build
cmake …
make
./disparity
在这里插入图片描述
如果你出现了这张图片，那就是你的disparity.cpp中的图片位置没有写对，找不到图片所导致的！！

运行成功如下：
在这里插入图片描述

3.矩阵微分

在这里插入图片描述

①第一问：如果大家有不理解的地方可以看看这个印度三哥的视屏，我认为讲的还是非常清晰的，至少我搜了很多国内的都没有这个讲得好，虽然语言不通，但是一点都不影响学习。高博的清华PPT还是不适合我这种人看。
链接:
矩阵求导讲解
在这里插入图片描述

②第二问：如果大家有不理解的地方可以看看这个印度三哥的视屏，我认为讲的还是非常清晰的，至少我搜了很多国内的都没有这个讲得好，虽然语言不通，但是一点都不影响学习。高博的清华PPT还是不适合我这种人看。
链接:
矩阵求导讲解
在这里插入图片描述

③第三问：
在这里插入图片描述

4.高斯牛顿法的曲线拟合实验

在这里插入图片描述

当然我觉得大家很有必要了解一下这一块的由来，我的上一篇博客讲述了海斯矩阵，凸函数等基本概念，大家看这个之前我认为很必要学习一下： 链接:
SLAM第四讲实践中的最优化知识

在做这道题之前我们非常有必要了解一下什么是牛顿法法，因为高斯牛顿法是牛顿法的改进，我以一道最优化的简单立体让你明白什么是牛顿法：
在这里插入图片描述
在这里插入图片描述
下来我们再看高斯牛顿法，我搜查了很多资料，很难找到一道高斯牛顿法的数学题来让大家理解，所以我只能找到一个更为详细点的高斯牛顿法的计算步骤让大家理解：
在这里插入图片描述

到这里，我们开始做题：
gaussnewton.cpp：

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
 
using namespace std;
using namespace Eigen;
 
int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器
 
  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }
 
  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
 
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {
 
    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;
 
    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);//计算雅可比矩阵J（Xk）和误差f(Xk)
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc
 
      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;
 
      cost += error * error;
    }
 
    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
 
    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }
 
    ae += dx[0];
    be += dx[1];
    ce += dx[2];
 
    lastCost = cost;
 
    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }
 
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
 
  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

CMakeLists.txt：

cmake_minimum_required(VERSION 2.8)
project(homework4)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")

add_executable(homework4 gaussnewton.cpp)
target_link_libraries(homework4 ${OpenCV_LIBS})

然后老五套
mkdir build
cd build
cmake …
make
./homework4
在这里插入图片描述

在这里插入图片描述

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

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

相关文章

excle快速填充公式等

excle快速填充公式等

阅读更多...
【性能】如何计算 Web 页面的 FP 指标

【性能】如何计算 Web 页面的 FP 指标

什么是 FP 指标 FP (First Paint) 为首次渲染的时间点&#xff0c;在性能统计指标中&#xff0c;从用户开始访问 Web 页面的时间点到 FP 的时间点这段时间可以被视为 白屏时间&#xff0c;也就是说在用户访问 Web 网页的过程中&#xff0c;FP 时间点之前&#xff0c;用户看到的…
阅读更多...
linux 网络 cat /proc/net/dev 查看测试网络丢包情况

linux 网络 cat /proc/net/dev 查看测试网络丢包情况

可以通过 cat /proc/net/dev 查看测试网络丢包情况&#xff0c;drop关键字&#xff0c;查看所有网卡的丢包情况 还可以看其他数据&#xff0c; /proc/net/下面有如下文件
阅读更多...
CSS英文单词强制截断换行

CSS英文单词强制截断换行

效果图&#xff1a; 代码&#xff1a; <!DOCTYPE html> <html> <head> <meta charset"utf-8"> <title>菜鸟教程(runoob.com)</title> <style> p.test1 {width:9em; border:1px solid #000000;word-break:keep-all; }p.…
阅读更多...
SchedulingConfigurer教程，怎么使用Spring自带的可扩展定时任务调度接口

SchedulingConfigurer教程，怎么使用Spring自带的可扩展定时任务调度接口

简介&#xff1a; SchedulingConfigurer 是 Spring 框架中的一个接口&#xff0c;用于配置任务调度&#xff08;scheduling&#xff09;的相关设置。在 Spring 中&#xff0c;任务调度通常通过 Spring 的任务调度模块&#xff08;Task Scheduling&#xff09;来实现&#xff0c…
阅读更多...
Unity优化（1）——合并Mesh

Unity优化（1）——合并Mesh

在某些移动端项目中&#xff0c;对于DrawCall的要求是很严格的&#xff0c;我们一般查看DrawCall可以通过Statistics里面的Batches进行查看&#xff0c;一般移动设备的Batches要控制在200左右比较合适&#xff0c;所以降低Batches是很重要的。 我们常常会遇到一个物体下挂载很多…
阅读更多...
记录我常用的免费API接口

记录我常用的免费API接口

目录 1.随机中英文句子 2.随机中英文句子&#xff08;带图片和音频&#xff09; 3.随机一句诗 4.随机一句话 5.随机一句情话 6. 随机一句舔狗语录 7.历史上的今天 8.获取来访者ip地址 9&#xff1a;获取手机号信息 10. 垃圾分类查询 11.字典查询 12.QQ信息查询 1.随…
阅读更多...
一文图解爬虫_姊妹篇（spider）

一文图解爬虫_姊妹篇（spider）

—引导语 爬虫&#xff0c;没有一个时代比当前更重视它。一个好的爬虫似乎可以洞穿整个互联网&#xff0c;“来装满自己的胃”。 接上一篇&#xff1a;一文图解爬虫&#xff08;spider&#xff09; 博主已初步对爬虫的“五脏六腑”进行了解剖。虽然俗称“爬虫”&#xff0c;但窃…
阅读更多...
设计测试用例的6种基本原则

设计测试用例的6种基本原则

设计测试用例的基本原则&#xff0c;对于软件测试非常重要&#xff0c;这些原则有助于设计出高质量、全面、有效的测试用例&#xff0c;从而提高软件测试的效率和准确性&#xff0c;维护软件的质量和稳定。如果在设计用例时没有遵循基本原则&#xff0c;这会影响用例的全面性、…
阅读更多...
【考研数据结构代码题6】构建二叉树及四大遍历（先中后层）

【考研数据结构代码题6】构建二叉树及四大遍历（先中后层）

题目&#xff1a;请你编写完整的程序构建一棵二叉树并对其进行先序遍历、中序遍历、后序遍历与层次遍历&#xff0c;分别打印并输出遍历结果 难度&#xff1a;★★★ 二叉树的存储结构 typedef struct Node{char data;//数据域struct Node* left;//左子树struct Node* right;//…
阅读更多...
module pandas has no attribute Int64Index

module pandas has no attribute Int64Index

pandas报错 pandas 报错解决 pandas 报错 module pandas has no attribute Int64Index 解决 将pandas将为1.1.3版本即可pip uninstall pandas pip install pandas1.1.3 -i https://pypi.tuna.tsinghua.edu.cn/simple/
阅读更多...
跌破1940后金价直指1900 对黄金代理是好是坏？

跌破1940后金价直指1900 对黄金代理是好是坏？

受以鲍威尔为首的美联储官员近期讲话的影响&#xff0c;加上巴以冲突暂时出现降温&#xff0c;导致避险需求下降&#xff0c;在两大因素的影响之下&#xff0c;现货黄金行情在近期的大涨之后出现大跌。金价不光跌破1950关口&#xff0c;在跌穿1940后势头更是直指1900。金价在一…
阅读更多...
程序员兼职平台有哪些？这样写兼职简历让你收入直接飙升30k！

程序员兼职平台有哪些？这样写兼职简历让你收入直接飙升30k！

这是著名程序员兼职平台 “猿急送”的分享资料&#xff0c;猿急送2015年成立&#xff0c;国内最早最领先的程序员兼职平台&#xff0c;今天跟大家继续讲一下在程序员兼职平台里接单的一个重要问题&#xff1a;怎样写好个人兼职简历&#xff1f;&#xff08;想直接看主流程序员兼…
阅读更多...
LeetCode反转链表的五种Java实现方式

LeetCode反转链表的五种Java实现方式

给你单链表的头节点 head &#xff0c;请你反转链表&#xff0c;并返回反转后的链表。 示例 1&#xff1a; 输入&#xff1a;head [1,2,3,4,5] 输出&#xff1a;[5,4,3,2,1]示例 2&#xff1a; 输入&#xff1a;head [1,2] 输出&#xff1a;[2,1]示例 3&#xff1a; 输入&a…
阅读更多...
简单理解路由重分发（用两路由器来理解）

简单理解路由重分发（用两路由器来理解）

相关命令&#xff1a; default-information originate //*重分发默认路由 redistribute rip subnets //*重分发rip redistribute ospf 1 metric 3 //*重分发ospf&#xff08;其中&#xff1a;1是ospf进程id 3是跳数&#xff09; redistribute sta…
阅读更多...
2024年春季3月退役的大学生士兵免试专升本单独报名的新政策

2024年春季3月退役的大学生士兵免试专升本单独报名的新政策

关于2024年春季3月退役大学生士兵专升本免试单独报名安排的通知 2024年3月退役的符合条件的大学生士兵单独组织一次报名&#xff0c;网上报名时间另行通知&#xff0c;履行网上报名和信息确认手续&#xff0c;根据要求上传本人头像照片、身份证照片&#xff0c;以及《入伍通知书…
阅读更多...
电机应用-控制系统、PID

电机应用-控制系统、PID

控制系统 对生产中某些关键性参数进行自动控制&#xff0c;使它们在受到外界干扰&#xff08;扰动&#xff09;的影响而偏离正常状态时&#xff0c;能够被自动地调节而回到工艺所要求地数值范围内。 自动控制系统分为&#xff1a;开环、闭环。 闭环自动控制系统原理 闭环控制是…
阅读更多...
threejs (四) 纹理 Texture

threejs (四) 纹理 Texture

定义&#xff1a;纹理图片&#xff08;或canvas/video等&#xff09;映射到物体表面&#xff0c;或者作为反射、折射贴图&#xff0c;也就是物体的皮肤。 1、纹理贴图分类 map&#xff1a;颜色贴图&#xff0c;存储颜色信息bumpMap&#xff1a;凹凸贴图&#xff0c;性能贴图&…
阅读更多...
翻牌器特效--vue3 封装组件

翻牌器特效--vue3 封装组件

1.效果图 2.下面为封装好的代码&#xff0c;在页面中引入即可 html <template><div id"flip-container" v-if"flag false"><div id"digit-1"class"digit">0</div><div id"digit-2"class"…
阅读更多...
算法笔记—第五章-最大公约数与最小公倍数

算法笔记—第五章-最大公约数与最小公倍数

算法笔记-最大公约数与最小公倍数 最大公约数最小公倍数最大公约数 2最小公倍数2互质互质2 最大公约数 //最大公约数与最小公倍数#include <cstdio> int gcd(int a, int b) {if (b 0) //截止的条件{return a; }else {return gcd(b, a % b);//这里是a与b和b&#xff…
阅读更多...
最新文章

Copyright @ 2022~2023