OpenCV區域性座標系和世界座標系轉換
本文實現區域性座標系和世界座標系的轉換。
1.區域性座標系->世界座標系
假設機器在p1處在世界座標系下的位姿為(x,y,z,θ)=(1,1,0,0),p1 和p2之間的轉換(區域性座標系)為t12表示為(1,1,0,45),
求p2處世界座標系下的位姿(x,y,z,θ)?
結果為(2,2,0,45)
程式如下:
#include<opencv2/opencv.hpp>
#include<iostream>
using namespace std;
using namespace cv;
void set_pose_by_R_t( Mat R, Mat t,Mat& T)
{
Mat tmpT = cv::Mat::eye(4 ,4,R.type());
R.copyTo(tmpT.rowRange(0,3).colRange(0,3));
t.copyTo(tmpT.rowRange(0,3).col(3));
T=tmpT.clone();
}
void get_pose_R_t(Mat T,Mat& R, Mat& t)
{
R=Mat::zeros(3,3,T.type());
t=Mat::zeros(3,1,T.type());
T.rowRange(0,3).colRange(0,3).copyTo(R);
T.rowRange(0,3).col(3 ).copyTo(t);
}
int main()
{
//1.p1 world position expression by opencv
cv::Mat p1rvec = (cv::Mat_<float>(3, 1) << 0,0,0);
Mat p1R;
Rodrigues(p1rvec*CV_PI / 180,p1R);
cv::Mat p1t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat p1T;
set_pose_by_R_t(p1R,p1t,p1T);
cout <<p1T<<endl;
//2. t12 value
cv::Mat t12rvec = (cv::Mat_<float>(3, 1) << 0,0,45);
Mat t12R;
Rodrigues(t12rvec*CV_PI / 180,t12R);
cv::Mat t12t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat t12T;
set_pose_by_R_t(t12R,t12t,t12T);
cout<<t12T<<endl;
//3.calculate p2 world positon by opencv
//3.1 calculate by 3*3 matrix p2Rcv=p1R*t12R;; p2tcv=p1R*t12t+t12t;
Mat p2Rcv=p1R*t12R;
Mat p2tcv=p1R*t12t+t12t;
Mat p2Tcv;
set_pose_by_R_t(p2Rcv,p2tcv,p2Tcv);
cout<<p2Tcv<<endl;
Mat p2rvec;
Rodrigues(p2Rcv, p2rvec);
p2rvec=p2rvec*180/ CV_PI;
cout<<"p2 R"<<p2rvec<<endl;
cout<<"p2 t"<<p2tcv<<endl;
//3.1 calculate by 4*4 matrix p2T=p1T*T12;
Mat p2T=p1T*t12T;
Mat p2Rcv2;
Mat p2tcv2;
get_pose_R_t(p2T,p2Rcv2,p2tcv2);
cout<<"4*4T"<<p2T<<endl;
Mat p2rvec2;
Rodrigues(p2Rcv2, p2rvec2);
p2rvec2=p2rvec2*180/ CV_PI;
cout<<"4*4 p2 R"<<p2rvec2<<endl;
cout<<"4*4 p2 t"<<p2tcv2<<endl;
return 0;
}
2.世界座標系->區域性座標系
假設機器在p1處在世界座標系下的位姿為(x,y,z,θ)=(1,1,0,0),p2處世界座標系下的位姿(x,y,z,θ)=(2,2,0,45)
求p1 和p2之間的轉換t12。
程式如下:
#include<opencv2/opencv.hpp>
#include<iostream>
using namespace std;
using namespace cv;
void set_pose_by_R_t( Mat R, Mat t,Mat& T)
{
Mat tmpT = cv::Mat::eye(4,4,R.type());
R.copyTo(tmpT.rowRange(0,3).colRange(0,3));
t.copyTo(tmpT.rowRange(0,3).col(3));
T=tmpT.clone();
}
void get_pose_R_t(Mat T,Mat& R, Mat& t)
{
R=Mat::zeros(3,3,T.type());
t=Mat::zeros(3,1,T.type());
T.rowRange(0,3).colRange(0,3).copyTo(R);
T.rowRange(0,3).col(3).copyTo(t);
}
int main()
{
//1.p1 world position expression by opencv
cv::Mat p1rvec = (cv::Mat_<float>(3, 1) << 0,0,0);
Mat p1R;
Rodrigues(p1rvec*CV_PI / 180,p1R);
cv::Mat p1t = (cv::Mat_<float>(3, 1) << 1, 1, 0);
Mat p1T;
set_pose_by_R_t(p1R,p1t,p1T);
cout<<p1T<<endl;
//2.p2 world position
cv::Mat p2rvec = (cv::Mat_<float>(3, 1) << 0,0,45);
Mat p2R;
Rodrigues(p2rvec*CV_PI / 180,p2R);
cv::Mat p2t = (cv::Mat_<float>(3, 1) << 2, 2, 0);
Mat p2T;
set_pose_by_R_t(p2R,p2t,p2T);
cout<<p2T<<endl;
//3.calculate t12
Mat t12T=p1T.inv()*p2T;
cout<<t12T<<endl;
Mat t12R;
Mat t12t;
get_pose_R_t(t12T,t12R,t12t);
Mat t12rvec;
Rodrigues(t12R, t12rvec);
t12rvec=t12rvec*180/ CV_PI;
cout<<"t12rvec"<<t12rvec<<endl;
cout<<"t12t"<<t12t<<endl;
return 0;
}
相關推薦
OpenCV區域性座標系和世界座標系轉換
本文實現區域性座標系和世界座標系的轉換。 1.區域性座標系->世界座標系 假設機器在p1處在世界座標系下的位姿為(x,y,z,θ)=(1,1,0,0),p1 和p2之間的轉換(區域性座標系)為t12表示為(1,1,0,45), 求p2處世界座標系
Eigen::區域性座標系和base座標系轉換
"越有故事的人越沉靜簡單,越膚淺單薄的人越浮躁不安!" 無論是做SLAM還是感測器標定,離不開的是座標之間的轉換關係 Eigen 一個開源的C++矩陣運算庫 perfect~ 言歸正傳: 使用Eigen庫進行空間座標的轉換。
Unity外掛之NGUI學習(9)—— Tween和世界座標系尺寸轉換為NGUI尺寸
在遊戲中,有一種比較常見的動畫效果,就是產生得分後,分數會在遊戲中顯示,並快速移動到總分的位置並使之相加。今天我就打算使用NGUI的Tween來製作這種分數動畫效果。 根據 Unity外掛之NGUI
Android感測器 裝置座標系到世界座標系的轉換 分析以及應用
首先分別介紹兩種座標系的不同之處: 世界座標系: Y軸:和地球表面正切,並且指向磁北極 Z軸:和地球表面垂直,然後指向地球的中心 X軸:和Y,Z垂直,並且幾乎是直接指向磁東 裝置座標系: 這個很好理解,垂直手機螢幕那一面向上為Z軸,向右為
工業現場相機座標系和機械手座標系的標定
原文:https://blog.csdn.net/kaychangeek/article/details/73878994 參考:https://blog.csdn.net/qq_16481211/article/details/79764730 工業現場使用視覺時一般需要相機座標系和機械手
opencv 中 IplImage和Mat相互轉換
Mat 轉 IplImage * IplImage imgTmp= (IplImage)(frame); IplImage *img= &imgTmp;//&:原來直接使用 &(IplImage)frame操作取了臨時變數的地址。返回後臨時變數已經
工業現場相機座標系和機械手座標系的標定(2)-相機和機械手繫結的情況
之前,我寫過一篇標定文章,但是隻說明了相機和機械手分離的情況的標定方法,為了提供完整的標定方案,這邊介紹一下怎麼利用我做的標定助手完成相機和機械手繫結的情況的標定方法。 由文章“工業現場相機座標系和機械手座標系的標定(1)-相機和機械手分離的
你必須知道的地理座標系和投影座標系
僅供參考,歡迎指正。 1、基本概念 地理座標系:為球面座標。 參考平面地是橢球面,座標單位:經緯度; 投影座標系:為平面座標。參考平面地是水平面,座標單位:米、千米等; 地理座標轉換到投影座標的過程可理解為投影。(投影:將不規則的地球曲面轉換為平面)
地理座標和投影座標 你必須知道的地理座標系和投影座標系
原 你必須知道的地理座標系和投影座標系 置頂 2017年03月24日 17:34:21 ConardLi 閱讀數:32428
多變數微積分筆記19——直角座標系和柱座標系下的三重積分
三重積分 三重積分由平面轉到了空間,但本質上與二重積分一致。f(x,y,z)是空間函式,對應的三重積分是: 其中R區域是f在定義域範圍內的圖形的體積,dv是體積積元。在二重積分中,面積積元dA =
地理座標系和投影座標系
地理座標系:為球面座標。 參考平面地是橢球面,座標單位:經緯度; 投影座標系:為平面座標。參考平面地是水平面,座標單位:米、千米等; 地理座標轉換到投影座標的過程可理解為投影。(投影:將不規則的地球曲面轉換為平面) 大地水準面 地球橢球體 大地基準面
慣性座標系、物體座標系、世界座標系
一、座標系的區別 1.世界座標系 世界座標系是一個特殊的座標系,它建立了描述其他座標系所需要的參考系。也就是說,可以用世界座標系去描述其他所有座標系或者物體的位置。所以有很多人定義世界座標系是“我們所關心的最大座標系”,通過這個座標系可以去描述和刻畫所
OpenCV學習之世界座標系、相機座標系、影象座標系和畫素座標系之間的轉換關係
一、各座標系介紹影象處理、立體視覺經常涉及到世界座標系、相機座標系、影象座標系和畫素座標系。如下圖所示:世界座標系是為了確定相機的位置,在雙目視覺中一般將世界座標系原點定在左相機、右相機或兩者X軸方向的中點。下面是各座標系之間的裝換,換而言之,就是一個現實中的物體是如何在影象
CGCS2000座標系與其他座標系間的差異和轉換方法
1954北京座標系和1980西安座標系是以天文大地網等經典測量技術為基礎的區域性座標系。 CGCS2000是以地球質量中心為原點的地心大地座標系。地心大地座標系可以滿足大地測量、地球物理、天文、導航和航天應用以及經濟、社會發展的廣泛需求。 其基本原則是: 座標系儘量對準ITRF(國際地球參考架
QT:百度座標(BD09)、國測局座標(火星座標,GCJ02)、和WGS84座標系之間的轉換(JS版程式碼)
/** * Created by Wandergis on 2015/7/8. * 提供了百度座標(BD09)、國測局座標(火星座標,GCJ02)、和WGS84座標系之間的轉換 */ //定義一些常量 var x_PI = 3.14159265358979324 * 3000.0 / 18
WGS84 GCJ02和BD09座標系相互轉換程式碼
var GPS = { PI : 3.14159265358979324, x_pi : 3.14159265358979324 * 3000.0 / 180.0, delta : function (lat, lon) { //
OpenCV和FFMpeg圖片轉換對比
最近一直在處理圖片,從H264解碼後得到的圖片是YUV圖片,而且很多都是NV12的,不是YUV420P(它們的差別是NV12格式為YYY...Y UV UV UV ... UV,而420P格式為 YYY...YY UUU..U VVV...V),一張1920x1080的圖片大小為3.1M,為了節省空間,我需要
各種經緯度座標系轉換-百度座標系、火星座標系、國際座標系
(文章程式碼參考網上 測試沒什麼問題, 彙總整理希望對大家有幫助-dou ) WGS84:國際座標系,為一種大地座標系,也是目前廣泛使用的GPS全球衛星定位系統使用的座標系。 GCJ02:火星座標系,是由中國國家測繪局制訂的地理資訊系統的座標系統。由WGS84座標系經加密後的座標系。 BD
OpenCV的Mat和Halcon的HObject型別互相轉換
專案之前一直使用的OpenCV,最近有個小問題使用Halcon有更好的效果,於是有了cv::Mat和Halcon中HObject互相轉換的需求。 參考了部落格http://blog.csdn.net/u010627377/article/details/76687
感測器座標系與車身座標系的轉換關係
文章目錄 1座標系的定義 1.1車身座標系定義如下: 1.2感測器座標系定義如下: 2座標系旋轉 3座標系平移 1座標系的定義 1.1車身座標系定義如下: 1.