g2o、Eigen、Mat矩阵类型转换
点云PCL免费知识星球,点云论文速读。
标题:g2o、Eigen、Mat矩阵类型转换
作者:Leather_Wang
来源:https://me.csdn.net/hzwwpgmwy
排版:particle
本文仅做学术分享,已获得作者授权转载,未经允许请勿二次转载!欢迎各位加入免费知识星球,获取PDF文档,欢迎转发朋友圈,分享快乐。
希望有更多的小伙伴能够加入我们,一起开启论文阅读,相互分享的微信群。参与和分享的方式:dianyunpcl@163.com
Eigen矩阵赋值
1) 使用row或者col
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
R.row(0) = Eigen::Vector3d(-0.0134899,-0.997066,0.0753502);
R.row(1) = Eigen::Vector3d(-0.0781018,-0.0740761,-0.99419);
R.row(2) = Eigen::Vector3d(0.996854,-0.0192965,-0.0768734);
2)使用block
注意:g2o中使用的g2o::Matrix3D是typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;,即还是使用的Eigen矩阵
g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);
Se3<->SE3Quat
Matrix3D Rbc=toEigenMatrix3d(se3bc.R());
Vector3D tbc=toG2oVector3D(se3bc.tvec);
g2o::SE3Quat Tbc=g2o::SE3Quat(Rbc, tbc);
Se2->SE3Quat
方法一:
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure.x, measure_se2.y, 0));
方法二:
g2o::SE3Quat toG2oSE3Quat(const g2o::SE2& se2)
{
g2o::Vector3D v2 = se2.toVector();
double x = v2(0);
double y = v2(1);
double theta = v2(2);
g2o::Matrix3D R = g2o::Matrix3D::Identity();
R.block<2,2>(0,0) << cos(theta),-sin(theta),sin(theta),cos(theta);
g2o::Vector3D t;
t << x,y,0;
return g2o::SE3Quat(R,t);
}
Mat->Eigen
Eigen::MatrixXd toEigenMatrixXd(const cv::Mat &cvMat)
{
Eigen::MatrixXd eigenMat;
eigenMat.resize(cvMat.rows, cvMat.cols);
for (int i=0; i<cvMat.rows; i++)
for (int j=0; j<cvMat.cols; j++)
eigenMat(i,j) = cvMat.at<float>(i,j);
return eigenMat;
}
Se3->Isometry3D
g2o::Isometry3D toG2oIsometry3D(const cv::Mat &T){
Eigen::Matrix<double,3,3> R;
R << T.at<float>(0,0), T.at<float>(0,1), T.at<float>(0,2),
T.at<float>(1,0), T.at<float>(1,1), T.at<float>(1,2),
T.at<float>(2,0), T.at<float>(2,1), T.at<float>(2,2);
g2o::Isometry3D ret = (g2o::Isometry3D) Eigen::Quaterniond(R);
Eigen::Vector3d t(T.at<float>(0,3), T.at<float>(1,3), T.at<float>(2,3));
ret.translation() = t;
return ret;
}
g2o::Isometry3D toG2oIsometry3D(const Se3& _se3) {
return toG2oIsometry3D(_se3.T());
}
Isometry3D与SE3Quat互相转换
1)SE3Quat 类型
SE3Quat 是g2o中老版本相机位姿的表示,内部使用四元数+平移向量存储位姿,同时支持李代数上的运算,例如对数映射(log函数)、李代数上增量(update函数)、指数映射(exp函数)、伴随矩阵(adj函数)等操作
g2o中定义顶点VertexSE3Expmap中的oplusImpl函数实现就是使用左乘更新
class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
virtual void oplusImpl(const double* update_) {
Eigen::Map<const Vector6d> update(update_);
// Vector6d update;
// update[1] = update_[1]; update[3] = update_[3]; update[5] = update_[5];
setEstimate(SE3Quat::exp(update)*estimate());
}
......
使用SE3Quat成员函数to_homogeneous_matrix()将SE3Quat->Eigen::Matrix<double,4,4>
2)Isometry3D->SE3Quat
/**
* convert an Isometry3D to the old SE3Quat class
*/
G2O_TYPES_SLAM3D_API SE3Quat toSE3Quat(const Isometry3D& t);
3)SE3Quat->Isometry3D
/**
* convert from an old SE3Quat into Isometry3D
*/
G2O_TYPES_SLAM3D_API Isometry3D fromSE3Quat(const SE3Quat& t);
4)Mat->Se3
注意对Mat操作使用_T.colRange(0,3).rowRange(0,3).clone()获取矩阵块
Se3::Se3(const cv::Mat &_T)
{
assert(_T.type() == CV_32FC1);
Mat R = _T.colRange(0,3).rowRange(0,3).clone();
Rodrigues(R, rvec);
tvec = _T.col(3).rowRange(0,3).clone();
}
5)Mat类型操作 copyTo和colRange使用
Mat Rcd = Mat::zeros(3,3,CV_32FC1);
rx.copyTo(Rcd.colRange(0,1));
ry.copyTo(Rcd.colRange(1,2));
rz.copyTo(Rcd.colRange(2,3));
- JavaScript 教程
- JavaScript 编辑工具
- JavaScript 与HTML
- JavaScript 与Java
- JavaScript 数据结构
- JavaScript 基本数据类型
- JavaScript 特殊数据类型
- JavaScript 运算符
- JavaScript typeof 运算符
- JavaScript 表达式
- JavaScript 类型转换
- JavaScript 基本语法
- JavaScript 注释
- Javascript 基本处理流程
- Javascript 选择结构
- Javascript if 语句
- Javascript if 语句的嵌套
- Javascript switch 语句
- Javascript 循环结构
- Javascript 循环结构实例
- Javascript 跳转语句
- Javascript 控制语句总结
- Javascript 函数介绍
- Javascript 函数的定义
- Javascript 函数调用
- Javascript 几种特殊的函数
- JavaScript 内置函数简介
- Javascript eval() 函数
- Javascript isFinite() 函数
- Javascript isNaN() 函数
- parseInt() 与 parseFloat()
- escape() 与 unescape()
- Javascript 字符串介绍
- Javascript length属性
- javascript 字符串函数
- Javascript 日期对象简介
- Javascript 日期对象用途
- Date 对象属性和方法
- Javascript 数组是什么
- Javascript 创建数组
- Javascript 数组赋值与取值
- Javascript 数组属性和方法
- Gradle的缓存路径修改的四种方法(小结)
- 适配AndroidQ拍照和读取相册图片的实现方法
- 这是我的第一篇文章
- Android自定义View实现五子棋游戏
- AndroidQ(10)黑暗模式适配的实现
- Android仿微信录音功能
- android实现截图并动画消失效果的思路详解
- 位运算判断奇偶数
- Kotlin之在Gradle中无参(no-arg)编译器插件的使用详解
- android实现常驻通知栏遇到的问题及解决办法
- Android仿微信键盘切换效果
- Android实现WIFI和GPRS网络的切换
- 在VS2010里快要疯掉的hello world
- gh0st源码分析与远控的编写(三)
- Android自定义View实现五星好评效果