【SLAM后端】—— ceres优化相机位姿求解
求解结果如下：
mat 初始化，eigenvalue初始化
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
Eigen::Matrix<float,3,1> vd_3d;v_3d << 3, 2, 1;
求解目标函数结构体构造与实例
struct CurveFittingCost {CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_x);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];//p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T x_p = p[0] / p[2];T y_p = p[1] / p[2];T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2d _point2d;const Point3f _point3d;};struct CurveFittingCost {CurveFittingCost(Point2f point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_y);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T predicted_x = p[0] / p[2] * camera_matrix(0,0) + camera_matrix(0,2);T predicted_y = p[1] / p[2] * camera_matrix(1,1) + camera_matrix(1,2);residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create(const Point2f point2d, const Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2f _point2d;const Point3f _point3d;};struct PnPCeres{PnPCeres(Point2f uv, Point3f xyz) : _uv(uv), _xyz(xyz) {}// 残差的计算template <typename T>bool operator() (const T *const camera, // 位姿参数，有6维T* residual) const // 残差{T p[3];T point[3];point[0] = T(_xyz.x);point[1] = T(_xyz.y);point[2] = T(_xyz.z);ceres::AngleAxisRotatePoint(camera, point, p);//计算RPp[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];T xp = p[0] / p[2];T yp = p[1] / p[2];//xp,yp是归一化坐标，深度为p[2]T u_ = xp*camera_matrix(0, 0) + camera_matrix(0, 2);T v_ = yp*camera_matrix(1, 1) + camera_matrix(1, 2);residual[0] = T(_uv.x) - u_;residual[1] = T(_uv.y) - v_;return true;}static ceres::CostFunction* Create(const Point2f uv, const Point3f xyz) {return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>(new PnPCeres(uv, xyz)));}const Point2f _uv;const Point3f _xyz;};
目标函数调用方式
struct CurveFittingCost {CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}template<typename T>bool operator() (const T *const camera_pose,T *residual)const{T point[3];point[0] = T(_point3d.depth_x);point[1] = T(_point3d.depth_x);point[2] = T(_point3d.depth_z);T p[3];ceres::AngleAxisRotatePoint(camera_pose, point, p);p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];//p[0] / p[2],p[1] / p[2] is norm coordinate，the real depth id p[2]T x_p = p[0] / p[2];T y_p = p[1] / p[2];T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));residual[0] = T(_point2d.x) - predicted_x;residual[1] = T(_point2d.y) - predicted_y;return true;}static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(new CurveFittingCost(point2d, point3d)));}const Point2d _point2d;const Point3f _point3d;
};
相关文章：
SPOJ 1811 LCS [后缀自动机]
题意： 求两个串的最大连续子串 一个串建SAM，另一个串在上面跑 注意如果走了Suffix Link，sum需要更新为t[u].val1 Suffix Link有点像失配吧，当前状态s走不了了就到Suffix Link指向的状态fa上去，fa是s的后缀所以是可行的…
图像卷积下非极大值抑制 Sobel 的实现
bool sobelOptaEdge(const cv::Mat& srcImage, cv::Mat& resultImage, int flag) {CV_Assert(srcImage.channels() 1);// 初始化sobel水平核因子cv::Mat sobelX (cv::Mat_<double>(3, 3) << 1, 0, -1,2, 0, -2, 1, 0, -1);// 初始化sebel垂直核因子cv::…
was unable to refresh its cache! status = Cannot execute request on any known server
出现这种错误是因为: Eureka服务注册中心也会将自己作为客户端来尝试注册它自己，所以我们需要禁用它的客户端注册行为。 在 yml中设置 eureka.client.register-with-eurekafalse eureka.client.fetch-registryfalse 但在服务端是要这是为false的，在客…
【C++】浅析析构函数（基类中）为什么要写成虚基类？
为什么有了虚析构函数，就能先调用子类的析构函数？ class A {virtual ~A(){} };class B : A {virtual ~B(){} };A *p new B(); delete p; 唯一差别是，每个析构函数结束时会自动（隐含地）调上父类的析构函数࿰…
Roberts 边缘检测
#include <opencv2/opencv.hpp> // roberts算子实现 cv::Mat roberts(cv::Mat srcImage) {cv::Mat dstImage srcImage.clone();int nRows dstImage.rows;int nCols dstImage.cols;for (int i 0; i < nRows - 1; i){for (int j 0; j < nCols - 1; j){// 根据公…
vector、map删除当前记录
map<string, string> sMap; map<string, string>::iterator iter; for(iter sMap.begin();iter ! sMap.end();/* iter */) {sMap.erase(iter); }注意下列错误表达：1. for(iter sMap.begin();iter ! sMap.end(); iter ) {sMap.erase(iter); } 错误原因…
1-2 postman工具简介
postman提供了一个多窗口和多选项卡页面用于发送和接受请求，postman努力保持整洁和灵活，提供更多的空间，满足用户的需要。他很简单，能满足大部分接口的测试，性价比特别高。如图所示： 1.侧边栏 postman的侧边…
【C++】重载运算符（一）
1.1 重载运算符特点 重载运算符本质上是一次函数调用 除了operator() 运算符调用外，其他重载运算符不能含有默认参数。 当重载的运算符是成员函数时，this绑定到左侧运算对象。成员运算符函数（显式）的参数数量比运算对象少一个。…
javaScript的调试（二）
2019独角兽企业重金招聘Python工程师标准>>> 一、Firebug Firebug是Firefox浏览器的调试工具，只要我们在Firefox中安装了Firebug应用，就可以按F12或右击鼠标开启调试 那么我们就先来看一下如何在Firefox中安装了Firebug应用，一图剩…
Prewitt 边缘检测
#include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include "opencv2/imgproc/imgproc.hpp" #include <iostream> // prewitt算子实现 cv::Mat prewitts(cv::Mat img, bool verFlag false) {img.convertTo(img, CV_32FC…
[swift 进阶]读书笔记-第十一章：互用性 C11P1 实践:封装 CommonMark
第十一章：互用性 Interoperability 前言： swift 的最大优点就是与C 或者 OC 混编的时候稳的一匹 本章主要讲了swift和C之间的一些知识点。 11.1 实践:封装 CommonMark Hands-On: Wrapping CommonMark 这一小节更像是一个教程。教你如何封装C语言中的Comm…
【Cmake】Cmake学习记录
Cmake学习记录 1.1 常例 add_library(gen_reference_infogen_reference_info/gen_reference_info.hgen_reference_info/gen_reference_info.cc ) target_link_libraries(gen_reference_infoparamsimage_preprocessorcenter_extractorremapperdescriptor_generator${OpenCV_LI…
MySQL（三）用正则表达式搜索
正则表达式是用来匹配文本的特殊的串（字符集合），将一个模式（正则表达式）与一个文本串进行比较； 所有种类的程序设计语言、文本编辑器、操作系统等都支持正则表达式，正则表达式用正则表达式语言来…
计算梯度幅值与方向
Mat magX Mat(src.rows, src.cols, CV_32F); Mat magY Mat(src.rows, src.cols, CV_32F); Sobel(image, magX, CV_32F, 1, 0, 3); Sobel(image, magY, CV_32F, 0, 1, 3); // 计算斜率 Mat slopes Mat(image.rows, image.cols, CV_32F); divide(magY, magX, slopes); // 计…
好程序员web前端技术分享媒体查询
为什么80%的码农都做不了架构师？>>> 好程序员web前端技术分享媒体查询 什么是媒体查询 媒体查询可以让我们根据设备显示器的特性（如视口宽度、屏幕比例、设备方向：横向或纵向）为其设定CSS样式，媒体查询由媒…
【C++】重载运算符（二）
1.4 下标运算符p501 下标运算符必须是成员函数，表示容器的类通常可以通过容器中的位置访问元素，定义下标运算符operator[]一个包含下标运算符的类，通常，定义2个版本：一个返回普通引用，另一个是类的常量成员…
手动建库11.2.0.4
环境： oracle11.2.0.4 redhat6.2 在上篇文章中，我们只安装了oracle，还没有建立实例，本篇文章就来介绍如果手动建立实例。 1.创建密码文件（password file）----非必要 cd $ORACLE_HOME/dbs/ 查看是否有init.o…
滞后阈值边缘连接
// 边缘连接 void followEdges(int x, int y, Mat &magnitude, int tUpper,int tLower, Mat &edges) { edges.at<float>(y, x) 255; for (int i -1; i < 2; i) {for (int j -1; j < 2; j) {// 边界限制if((i ! 0) && (j ! 0) && (…
《复联4》的这波操作，其实是在灭 bug
前方涉及剧透，请谨慎阅读！！！前方涉及剧透，请谨慎阅读！！！前方涉及剧透，请谨慎阅读！！！↓↓↓Q1:您是否看了《复联4》？A、已看…
【C++】容器与继承
容器与继承 Exercise 15.28: Define a vector to hold Quote objects but put Bulk_quote objects into that vector. Compute the total net_price of all the elements in the vector. Exercise 15.29: Repeat your program, but this time store shared_ptrs to objects…
1.3 使用jmeter进行http接口测试
来源： http://www.cnblogs.com/alisapan/p/6150309.html 此篇纯是搬运记载。。 一、测试需求描述 1、 本次测试的接口为http服务端接口 2、 接口的主要分成两类，一类提供给查询功能接口，一类提供保存数据功能接口，这里我们举例2个…
opencv 自带库Canny边缘检测
#include "opencv2/imgproc/imgproc.hpp" #include "opencv2/highgui/highgui.hpp" int main( ) {// 读取源图像并转化为灰度图像cv::Mat srcImage cv::imread("..\\images\\flower.jpg",0);// 判断文件是否读入正确if( !srcImage.data ) retur…
git branch
git basic branch git branch <branchName>#create branch git branch --list#show branch list, git checkout <branchName>#choose branch,HEAD is pointer of the current branch last commit 复制代码create a file in branch 0.0.1,then checkout master ,mer…
读书笔记 effective c++ Item 5 了解c++默认生成并调用的函数
1 编译器会默认生成哪些函数 什么时候空类不再是一个空类？答案是用c处理的空类。如果你自己不声明，编译器会为你声明它们自己版本的拷贝构造函数，拷贝赋值运算符和析构函数，如果你一个构造函数都没有声明，编译器同样…
git tag学习记录(二)
文章目录1. git 工原理示意2. git tag记录2.1 git tag查看已有tag列表2.2 git tag标记当前分支上的 tag信息为-a v1.5.4 -m（给指定的commit打Tag）2.3 git push origin v1.5.4推送上一步打tag的分支到远程1. git 工原理示意 工作流程 TortoiseGit 2. g…
最小矩形与圆生成
#include "opencv2/highgui/highgui.hpp" #include "opencv2/imgproc/imgproc.hpp" #include <iostream> using namespace cv; using namespace std; int main() {cv::Mat img(500, 500, CV_8UC3);// 随机生成因子RNG& rng theRNG();for (;;){i…
详解微信域名防封的方法以及检测等工具的技术原理
微信域名完全防封是绝对不可能的，这是必须明确的，曾经有人打折《不死域名》的概念，它不是不死，是稍微命长一点，在推广上成本更低一下，效果更好一些，主要的技术原理是利用了腾讯云的域名安全联盟…
贝叶斯定理——数学之美
1.贝叶斯定理 1.1 定义：描述在已知一些条件下，某事件的发生概率 贝叶斯定理是关于随机事件A和B的条件概率的一则定理。 1.2 公式理解 P(x∣y)P(x)P(y∣x)P(y)P(x|y) \frac{ P(x)P(y|x)}{P(y)}P(x∣y)P(y)P(x)P(y∣x) 其中x以及y为随机事件ÿ…
HUST 1586 数字排列
1586 - 数字排列 时间限制：1秒 内存限制：128兆 91 次提交 36 次通过 题目描述现有n个k位的数字，你的任务是重新安排数字每一位的位置，使得重新安排后这n个数字中最大的数字和最小的数字之差的绝对值最小，对于每一位的调…
最小矩形与圆生成1
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <iostream> #include <stdio.h> using namespace cv; using namespace std; // 计算外接矩形与圆轮廓…