提取Eigen中Matrix元素等基本操作:Identity()函数、head<n>()函数

本文介绍了Eigen库中矩阵操作的几个关键点,包括如何正确提取矩阵特定元素,使用Identity()函数初始化单位矩阵,以及head&lt;n&gt;()函数用于提取向量前n个元素的方法。

1.对一个Eigen::Matrix3f类型的数据x,在想提取其第三行第一列元素时:

(1)使用x.row(2).col(0)操作并赋值给一个变量,提示不能将Eigen...类型值赋值过去;

(2)使用x(2,0)操作可以完成提出元素并赋值。

 

2.使用Identity()函数的作用:在定义变量时使用Eigen::Matrix4f x =  Eigen::Matrix4f::Identity();即用单位矩阵对x变量进行了初始化。(参考了https://blog.youkuaiyun.com/yang_q_x/article/details/52383289https://blog.youkuaiyun.com/caomin1hao/article/details/81358911?utm_source=blogxgwz5

 

3.head<n>()函数:对变量Eigen::Vector4f x进行x.head<n>()操作表示提取前n个元素。

 

 

// WES-ICP优化函数 Eigen::Matrix4f WES_ICP_Optimization( const pcl::PointCloud<pcl::PointXYZ>& source, const pcl::PointCloud<pcl::PointXYZ>& inter_points, const pcl::PointCloud<pcl::PointXYZ>& target, const pcl::PointCloud<pcl::Normal>& target_normals, const Eigen::VectorXf& d, double w_pTp, double w_pTpln) { Eigen::MatrixXf A = Eigen::MatrixXf::Zero(6, 6); Eigen::VectorXf b = Eigen::VectorXf::Zero(6); for (size_t i = 0; i < source.size(); ++i) { const auto& pt = source[i].getVector3fMap(); const auto& inter_pt = inter_points[i].getVector3fMap(); const auto& target_pt = target[i].getVector3fMap(); const auto& normal = target_normals[i].getNormalVector3fMap(); // 构建hat算子矩阵 Eigen::Matrix4f hat; hat << 0, -pt.z(), pt.y(), 1, pt.z(), 0, -pt.x(), 1, -pt.y(), pt.x(), 0, 1, 0, 0, 0, 0; // 点对点部分 Eigen::Vector4f delta_ptp(pt.x() - inter_pt.x(), pt.y() - inter_pt.y(), pt.z() - inter_pt.z(), 0); A += w_pTp * hat.transpose() * hat; b -= w_pTp * hat.transpose() * delta_ptp; // 点对面部分 Eigen::Vector4f n(normal.x(), normal.y(), normal.z(), 0); double delta_ptpln = (pt - target_pt).dot(normal) - d(i); A += w_pTpln * n.transpose() * hat.transpose() * hat * n; b -= w_pTpln * delta_ptpln * hat.transpose() * n; } // 解线性系统 Eigen::VectorXf x = A.ldlt().solve(b); // 指数映射到SE(3) Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); double angle = x.head<3>().norm(); if (angle > 1e-10) { T.block<3, 3>(0, 0) = Eigen::AngleAxisf(angle, x.head<3>().normalized()).toRotationMatrix(); } T.block<3, 1>(0, 3) = x.tail<3>(); return T; } 这个代码输出结果为空,请修改正确
07-02
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值