【计算机视觉】matlab编程实现根据PNP求解位姿（有问题版）

最新推荐文章于 2022-04-07 22:49:34 发布
最新推荐文章于 2022-04-07 22:49:34 发布
阅读量5.3k 收藏 6
点赞数 1
分类专栏： 三维重建 位姿测量 运动估计 视觉导航 算法学习
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/KYJL888/article/details/78828946
版权

应人邀请帮忙推导求解最小二乘的PNP估计位姿问题， matlab编程实现根据PNP求解时解貌似有问题？




clc;clear all;

n=4;
f=1000;

pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r21,r22,r23;r31,r32,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S2=M*[R,T]*[pw3(2,:)';1]-[pc2(2,:)';1]*zc;
S3=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S4=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;

    % solve(S1=0,S2=0,S3=0,S4=0);
    % solve('S1=0','S2=0','S3=0','S4=0');
    % [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve('S1=0','S2=0','S3=0','S4=0','','','');


% [r11,r12,r13,r22,r23,t1,t2,t3]=solve('1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '0*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000000*r11 - 100000*r12 + 1000*t1 - 41569/500','1000000*r12 - 100000*r22 + 1000*t2 - 10831/250',...
% '1000*r13 - 100*r23 + t3 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% ' t3 - 100*r23 - 1000*r13 - 1');

% 
% % 二
[r11,r12,r21,r22,r31,r32,t1,t2,t3,zc]=solve(...
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...  
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc',... 
' 1000000*r11 - 100000*r12 + 1000*t1 - (41569*zc)/500',... 
' 1000000*r21 - 100000*r22 + 1000*t2 - (10831*zc)/250',... 
'1000*r31 - 100*r32 + t3 - zc',... 
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
'1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc',... 
'  1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc');





clc;clear all;


n=4;
f=1000;


pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r21,r22,r23;r31,r32,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S2=M*[R,T]*[pw3(2,:)';1]-[pc2(2,:)';1]*zc;
S3=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S4=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;


    % solve(S1=0,S2=0,S3=0,S4=0);
    % solve('S1=0','S2=0','S3=0','S4=0');
    % [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve('S1=0','S2=0','S3=0','S4=0','','','');




% [r11,r12,r13,r22,r23,t1,t2,t3]=solve('1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '0*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000000*r11 - 100000*r12 + 1000*t1 - 41569/500','1000000*r12 - 100000*r22 + 1000*t2 - 10831/250',...
% '1000*r13 - 100*r23 + t3 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% ' t3 - 100*r23 - 1000*r13 - 1');


% 
% % 二
[r11,r12,r21,r22,r31,r32,t1,t2,t3,zc]=solve(...
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...  
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc',... 
' 1000000*r11 - 100000*r12 + 1000*t1 - (41569*zc)/500',... 
' 1000000*r21 - 100000*r22 + 1000*t2 - (10831*zc)/250',... 
'1000*r31 - 100*r32 + t3 - zc',... 
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
'1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc',... 
'  1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r32 - 1000*r31 - zc');



clc;clear all;
%% 该代码为验证迭代法求解位姿的程序 2017 12 15
n=4;
f=1000;


pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r22 r23 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r12,r22,r23;r13,r23,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S2=M*[R,T]*[pw3(2,:)';1]-[pc2(2,:)';1]*zc;
S3=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S4=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;


    % solve(S1=0,S2=0,S3=0,S4=0);
    % solve('S1=0','S2=0','S3=0','S4=0');
    % [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve('S1=0','S2=0','S3=0','S4=0','','','');




% [r11,r12,r13,r22,r23,t1,t2,t3]=solve('1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '0*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000000*r11 - 100000*r12 + 1000*t1 - 41569/500','1000000*r12 - 100000*r22 + 1000*t2 - 10831/250',...
% '1000*r13 - 100*r23 + t3 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% ' t3 - 100*r23 - 1000*r13 - 1');




% % 二
[r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve(...
'1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...  
'1000*t2 - 100000*r22 - 1000000*r12 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r23 - 1000*r13 - zc',... 
' 1000000*r11 - 100000*r12 + 1000*t1 - (41569*zc)/500',... 
' 1000000*r12 - 100000*r22 + 1000*t2 - (10831*zc)/250',... 
'1000*r13 - 100*r23 + t3 - zc',... 
'1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
'1000*t2 - 100000*r22 - 1000000*r12 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r23 - 1000*r13 - zc',... 
'1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',... 
' 1000*t2 - 100000*r22 - 1000000*r12 + (7932063359948135*zc)/140737488355328',... 
' t3 - 100*r23 - 1000*r13 - zc');



苏源流
关注
专栏目录
「3D视觉技术交流群」精华帖与关键问题
3D视觉工坊
07-27 3390
原文首发于微信公众号3D视觉工坊： 「3D视觉技术交流群」精华帖与关键问题 本文主要总结了「3D视觉技术交流群」自建立以来，星球内共发布与交流过的技术帖及关键问题，以便大家回顾温习。 一 星球主题，主要包括： 1.1　２Ｄ视觉 １）相机标定； ２）图像处理基础； ３）相机与镜头 1.2　手眼标定 1.3　３Ｄ视觉 1.4　编程技巧 1.5　VSLAＭ，包括：ubuntu系统；VSLAM技术； 1...
MATLAB算法实战应用案例精讲-【人工智能】基于机器视觉的机器人及机械臂运动规划（补充篇）
qq_36130719的博客
05-30 1268
随着工业的发展，在生产中对自动化的要求越来越高，视觉技术已被广泛引入工业(工业机械臂)机器人行业，具备视觉的工业(工业机械臂)机器人能更快、更准、更灵活地完成定位抓取、对位组装等。基于图像分析的视觉技术在(工业机械臂)机器人引导相关应用中的主要作用是精确获取对象物（待抓取物体）和目标物（待组装物体）的坐标位置和角度，并将图像坐标转换为(工业机械臂)机器人能识别的(工业机械臂)机器人坐标，指导(工业机械臂)机器人进行纠偏和组装。因此，手眼标定和定位引导是机器视觉在(工业机械臂)机器人引导中应用的的核心。
RPnP算法原文及代码解析
cg1135217680的博客
03-24 3454
（有不对的地方希望得到您的指正~~） RPnP（Robust Perspective-n-Point）是一种快速且具有鲁棒性的PnP求解方法。在2D-3D点较少的情况下，能取得较理想的计算效果。并且其结果远优于EPnP+GN，是目前解决PnP问题的最有效的方法之一。由于RPnP算法是在PST（Perspective Similar Triangle）算法之上，提出来的，故在解析RPnP前，需简单介绍一下PST算法。 PST算法是为了解决P3P问题，提出...
PnP位姿估计matlab工具包
09-03
最新的位姿估计工具箱，对应文献为：Very Fast Solution to the PnP Problem with Algebraic Outlier Rejection。PnP问题是计算机视觉中非常基础的经典问题，可以用来测量目标的三维位置姿态。
Matlab 的 PnP 方法的集合_包含我们的 SRPnP 和其他 PnP 算法_代码_下载
06-06
包含我们的 SRPnP 和其他 PnP 算法的 MATLAB 代码。 一种简单、稳健且快速的透视 n 点问题方法，
EPnP_matlab.rar_EPnP_MATLAB实现pnp_PnP求解_pnp算法matlab_用Matlab做pnp
07-15
通过设计四个虚拟控制点实现一种高效的PNP求解算法
PnP算法
热门推荐
陈建驱的博客
02-21 1万+
个人博客：http://www.chenjianqu.com/ 原文链接：http://www.chenjianqu.com/show-95.html PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。如果两张图像中其中一张特征点的 3D 位置已知，那么最少只需 3 个点对就可以使用PnP估计相机运动。特征点的 3D 位置可以由三角化或者 RGB-D ...
EPnP_matlab.rar_MATLAB实现pnp_matlab "epnp"_matlab 求解pnp_姿态_计算机视觉
09-24
EPnP算法的MATLAB实现，计算相机姿态
手眼标定Matlab程序,matlab画眼图,matlab
12-05
在这个Matlab程序中，我们主要关注如何利用计算机视觉和数学算法来实现这一过程。 手眼标定通常是为了让机器人能够理解来自摄像头的图像数据，并将这些数据转换为机器人工作空间中的坐标。这个过程涉及到多个坐标系...
机器视觉单目测量
06-14
在本项目中，作者使用MATLAB编程语言实现了高精度的单目测量算法。MATLAB因其强大的数学计算能力和丰富的图像处理工具箱，成为进行此类研究的常用平台。 单目测量的核心在于解决三维空间中的点到二维图像平面上的...
高斯牛顿迭代法matlab代码-pnpfr_problem:ECCV2016论文“解决PnP，PnPf和PnPfr问题的通用方法”的官方MAT
05-24
高斯牛顿继承法matlab代码解决PnP，PnPf和PnPfr问题的多功能方法 :copyright:2020 NEC公司 该存储库是ECCV2016论文“解决PnP，PnPf和PnPfr问题的通用方法”的官方MATLAB实现。 代码中使用的Gröbner基求解器由V. Larsson的多项式求解器自动生成器生成。 执照 该软件是根据NEC公司许可发布的。 使用代码之前，请参阅。 如果使用此代码，请引用本文。 @inproceedings { nakano2016versatile , title = { A versatile approach for solving PnP, PnPf, and PnPfr problems } , author = { Nakano, Gaku } , booktitle = { European Conference on Computer Vision } , pages = { 338--352 } , year = { 2016 } , organization = { Springer } } 对于商业用途，请联系中野学院（Gaku Nakano）。 用法
Matlab: rpnp
02-28
包含rpnp以及epnp算法的matlab程序，通过求取特征点旋转矩阵以及平移向量以获取摄像机的位姿信息。
PnP 算法简介 代码解析
01-10
PnP 算法简介 代码解析本期公开课将详细讲述常见的PnP求解算法。PnP求解算法是指通过多对3D与2D匹配点，在已知或者未知相机内参的情况下，利用最小化重投影误差来求解相机外参的算法。PnP求解算法是SLAM前端位姿跟踪部分中常用的算法之一，本次公开课，将详细讲述P3P、DLT、EPnP、UPnP、优化求解等多种常见的PnP求解算法。接下来，让我们一起深入学习PnP算法吧！
单目视觉的位姿估算算法
11-22
单目视觉的位姿估算算法 matlab代码 基于位姿估算正交迭代的算法，有test程序和主程序，test中可以比对估算的精度
MLPnP_matlab-master_pnp、最大似然_
10-01
这是最大似然n点透视的matlab代码。
视觉SLAM关键方法总结
计算机视觉life
05-21 1972
点“计算机视觉life”关注，置顶更快接收消息！ 最近在做基于激光信息的机器人行人跟踪发现如果单独利用激光信息很难完成机器人对行人的识别、跟踪等功能，因此考虑与视觉融合的方法，这样便可以充分利用激光雷达提供的精确位置信息及视觉提供的丰富纹理、颜色等场景信息。以下是最近调研视觉SLAM中的实现方法的总结，包括三方面内容：姿态计算、闭环检测、BA优化。 姿态计算 一、通过提取图像的特征描述子，如...
MATLAB标定工业相机及应用于PNP测距的注意事项
weixin_50067564的博客
04-07 2020
一、前言 因为项目采用PNP测距，需要知道工业相机的内参矩阵和畸变矩阵，所以采用MATLAB自带的标定应用CameraCalibrator进行标定工业相机（以迈德威视相机为例）。 原理及操作可结合以下博客： OpenCV 相机标定 （Python版） 二、准备 1、电脑上装好matlab。 2、普通无驱USB相机直接用，工业相机需要装SDK驱动，因为标定的时候需要拍照。 3、标准标定板-棋盘格图片，标定的时候保证平整， 请下载正规标定板PDF，以下仅为演示图片： 三、拍标定图 以下两点非常重要！！！！
ceres_solver解PnP平差问题、Matlab数据制作
LiChengwei的博客
11-21 2549
网上的PnP平差没有数据，很痛苦，自己制作数据来解 根据其次坐标的推导公式 在Matlab中制作数据(矩阵运算比较方便_) clc clear X = zeros(15, 3); x = zeros(15, 2); K=[25 0 0;0 25 0;0 0 1;]%相机内参数矩阵 R=[0.4770710827172032, -0.7476726229304006, -0.4619402893831741;%旋转矩阵 0.653281340399395, 0.653280157390419, -0.38
PnP问题求解
优秀的田的博客
04-09 3758
1.直接线性变换（DLT） 对某空间点P = (X, Y, Z, 1)T，在某图像中对应点为 x1 = (u1, v1, 1)T，则有 s(u1v11)=(t1t2t3t4t5t6t7t8t9t10t11t12)(XYZ1) s\left(\begin{array}{l} u_{1} \\ v_{1} \\ 1 \end{array}\right)=\left(\begin{array}{llll} t_{1} & t_{2} & t_{3} & t_{4} \\ t_{5} &am
matlab位姿解算
最新发布
10-22
Matlab位姿解算是指通过已知的机器人关节角度和DH参数，计算出机器人末端执行器的位姿（位置和姿态）。其中，DH参数是指机器人各个关节之间的距离、角度和长度等参数。在Matlab中，可以通过编写程序来实现位姿解算。常用的方法包括正运动学和逆运动学。其中，正运动学是指已知机器人关节角度和DH参数，计算出机器人末端执行器的位姿；逆运动学是指已知机器人末端执行器的位姿，计算出机器人关节角度和DH参数。在实际应用中，位姿解算可以用于机器人轨迹规划、运动控制等方面。 根据提供的引用内容，可以看出这是在进行机器人位姿解算的过程中，通过DH参数和关节角度计算出机器人末端执行器的变换矩阵。其中，变换矩阵T1表示机器人第一关节的变换矩阵，变换矩阵T2表示机器人第二关节的变换矩阵。这些变换矩阵可以用于计算机器人末端执行器的位姿。
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值