Eigen sparse 基本操作:构造 & 输出

本文展示了一个使用Eigen库创建和操作稀疏矩阵的C++示例。通过向稀疏矩阵中添加元素并处理重复索引的情况,演示了如何利用Eigen::Triplet进行矩阵填充。
#include <iostream>
#include "Eigen/Sparse"
int main()
{
	Eigen::SparseMatrix<float> m(3, 3);
	std::vector<Eigen::Triplet<float> > triple;
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			triple.push_back(Eigen::Triplet<float>(i, j, 0.1));
		}
	}
	triple.push_back(Eigen::Triplet<float>(1, 1, 0.1));////相同下标重复插入,则表示相加
	m.setFromTriplets(triple.begin(), triple.end());
	//std::cout << m << std::endl; //直接这样输出也是可以的
	for (int k = 0; k < m.outerSize(); ++k) {
		for (Eigen::SparseMatrix<float>::InnerIterator it(m, k); it; ++it)
		{
			//std::cout << it.row() << " " << it.col() << " : " << it.value() << std::endl;
			std::cout << it.value() << " ";
		}
		std::cout << std::endl;
	}
	return 0;
}

报错:guo@guo-Dell-G15-5520:~/g2o仿真/src/build$ make Consolidate compiler generated dependencies of target g2o_demo [ 50%] Building CXX object CMakeFiles/g2o_demo.dir/optimize.cc.o /home/guo/g2o仿真/src/optimize.cc: In function &lsquo;std::vector&lt;g2o::SE3Quat&gt; addNoise(const std::vector&lt;g2o::SE3Quat&gt;&amp;, double, double, bool)&rsquo;: /home/guo/g2o仿真/src/optimize.cc:90:61: error: no match for &lsquo;operator*&rsquo; (operand types are &lsquo;g2o::SE3Quat&rsquo; and &lsquo;const Quaternion&rsquo; {aka &lsquo;const Eigen::Quaternion&lt;double&gt;&rsquo;}) 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~~~ | | | | g2o::SE3Quat const Quaternion {aka const Eigen::Quaternion&lt;double&gt;} In file included from /home/guo/g2o仿真/src/optimize.cc:11: /opt/ros/noetic/include/g2o/types/slam3d/se3quat.h:99:22: note: candidate: &lsquo;g2o::SE3Quat g2o::SE3Quat::operatonst Eigen::MatrixBase&lt;U&gt;&amp;)&rsquo; 325 | operator*(const TranspositionsBase&lt;TranspositionsDerived&gt; &amp;transpositions, | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Core/Transpositions.h:325:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::TranspositionsBase&lt;TranspositionsDerived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Householder:24, from /usr/local/include/eigen3/Eigen/QR:15, from /usr/local/include/eigen3/Eigen/SVD:11, from /usr/local/include/eigen3/Eigen/Geometry:13, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Householder/HouseholderSequence.h:513:99: note: candidate: &lsquo;template&lt;class OtherDerived, class VectorsType, class CoeffsType, int Side&gt; typename Eigen::internal::matrix_type_times_scalar_type&lt;typename VectorsType::Scalar, OtherDerived&gt;::Type Eigen::operator*(const Eigen::MatrixBase&lt;Derived&gt;&amp;, const Eigen::HouseholderSequence&lt;VectorsType, CoeffsType, Side&gt;&amp;)&rsquo; 513 | typename internal::matrix_type_times_scalar_type&lt;typename VectorsType::Scalar,OtherDerived&gt;::Type operator*(const MatrixBase&lt;OtherDerived&gt;&amp; other, const HouseholderSequence&lt;VectorsType,CoeffsType,Side&gt;&amp; h) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Householder/HouseholderSequence.h:513:99: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::MatrixBase&lt;Derived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Geometry:46, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Geometry/Scaling.h:135:1: note: candidate: &lsquo;template&lt;class Derived, class Scalar&gt; Eigen::CwiseBinaryOp&lt;Eigen::internal::scalar_product_op&lt;typename Eigen::internal::traits&lt;T&gt;::Scalar, Scalar&gt;, const Derived, const typename Eigen::internal::plain_constant_type&lt;Expr, Scalar&gt;::type&gt; Eigen::operator*(const Eigen::MatrixBase&lt;Derived&gt;&amp;, const Eigen::UniformScaling&lt;Scalar&gt;&amp;)&rsquo; 135 | operator*(const MatrixBase&lt;Derived&gt;&amp; matrix, const UniformScaling&lt;Scalar&gt;&amp; s) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/Scaling.h:135:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::MatrixBase&lt;Derived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:147:1: note: candidate: &lsquo;template&lt;class SparseDerived, class PermDerived&gt; const Eigen::Product&lt;MatrixDerived, PermutationDerived, 2&gt; Eigen::operator*(const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&amp;, const Eigen::PermutationBase&lt;PermutationDerived&gt;&amp;)&rsquo; 147 | operator*(const SparseMatrixBase&lt;SparseDerived&gt;&amp; matrix, const PermutationBase&lt;PermDerived&gt;&amp; perm) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:147:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:154:1: note: candidate: &lsquo;template&lt;class SparseDerived, class PermDerived&gt; const Eigen::Product&lt;PermDerived, SparseDerived, 2&gt; Eigen::operator*(const Eigen::PermutationBase&lt;PermutationDerived&gt;&amp;, const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&amp;)&rsquo; 154 | operator*( const PermutationBase&lt;PermDerived&gt;&amp; perm, const SparseMatrixBase&lt;SparseDerived&gt;&amp; matrix) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:154:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::PermutationBase&lt;PermutationDerived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:162:1: note: candidate: &lsquo;template&lt;class SparseDerived, class PermutationType&gt; const Eigen::Product&lt;SparseDerived, Eigen::Inverse&lt;Rhs&gt;, 2&gt; Eigen::operator*(const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&amp;, const Eigen::InverseImpl&lt;PermutationType, Eigen::PermutationStorage&gt;&amp;)&rsquo; 162 | operator*(const SparseMatrixBase&lt;SparseDerived&gt;&amp; matrix, const InverseImpl&lt;PermutationType, PermutationStorage&gt;&amp; tperm) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:162:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/SparseCore:62, from /usr/local/include/eigen3/Eigen/Sparse:26, from /opt/ros/noetic/include/g2o/solvers/eigen/linear_solver_eigen.h:30, from /home/guo/g2o仿真/src/optimize.cc:10: /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:171:1: note: candidate: &lsquo;template&lt;class SparseDerived, class PermutationType&gt; const Eigen::Product&lt;Eigen::Inverse&lt;Rhs&gt;, SparseDerived, 2&gt; Eigen::operator*(const Eigen::InverseImpl&lt;PermutationType, Eigen::PermutationStorage&gt;&amp;, const Eigen::SparseMatrixBase&lt;OtherDerived&gt;&amp;)&rsquo; 171 | operator*(const InverseImpl&lt;PermutationType,PermutationStorage&gt;&amp; tperm, const SparseMatrixBase&lt;SparseDerived&gt;&amp; matrix) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h:171:1: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::InverseImpl&lt;PermutationType, Eigen::PermutationStorage&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/local/include/eigen3/Eigen/Geometry:40, from /home/guo/g2o仿真/src/optimize.cc:5: /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:80:66: note: candidate: &lsquo;Eigen::Transform&lt;double, 3, 2&gt; Eigen::operator*(const Eigen::DiagonalMatrix&lt;double, 3&gt;&amp;, const Eigen::Quaternion&lt;double&gt;&amp;)&rsquo; 80 | EIGEN_DEVICE_FUNC friend inline Transform&lt;Scalar,Dim,Affine&gt; operator*(const DiagonalMatrix&lt;Scalar,Dim&gt;&amp; l, const Derived&amp; r) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:80:110: note: no known conversion for argument 1 from &lsquo;g2o::SE3Quat&rsquo; to &lsquo;const Eigen::DiagonalMatrix&lt;double, 3&gt;&amp;&rsquo; 80 | EIGEN_DEVICE_FUNC friend inline Transform&lt;Scalar,Dim,Affine&gt; operator*(const DiagonalMatrix&lt;Scalar,Dim&gt;&amp; l, const Derived&amp; r) | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:76:49: note: candidate: &lsquo;template&lt;class OtherDerived&gt; Eigen::RotationBase&lt;Eigen::Quaternion&lt;double&gt;, 3&gt;::RotationMatrixType Eigen::operator*(const Eigen::EigenBase&lt;Derived&gt;&amp;, const Eigen::Quaternion&lt;double&gt;&amp;)&rsquo; 76 | EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase&lt;OtherDerived&gt;&amp; l, const Derived&amp; r) | ^~~~~~~~ /usr/local/include/eigen3/Eigen/src/Geometry/RotationBase.h:76:49: note: template argument deduction/substitution failed: /home/guo/g2o仿真/src/optimize.cc:90:81: note: &lsquo;g2o::SE3Quat&rsquo; is not derived from &lsquo;const Eigen::EigenBase&lt;Derived&gt;&rsquo; 90 | noisy[i].setRotation(g2o::SE3Quat::exp(rotNoiseVec) * noisy[i].rotation()); | ^ In file included from /usr/include/x86_64-linux-gnu/c++/9/bits/c++allocator.h:33, from /usr/include/c++/9/bits/allocator.h:46, from /usr/include/c++/9/string:41, from /usr/include/c++/9/bits/locale_classes.h:40, from /usr/include/c++/9/bits/ios_base.h:41, from /usr/include/c++/9/ios:42, from /usr/include/c++/9/ostream:38, from /usr/include/c++/9/iostream:39, from /home/guo/g2o仿真/src/optimize.cc:1: /usr/include/c++/9/ext/new_allocator.h: In instantiation of &lsquo;void __gnu_cxx::new_allocator&lt;_Tp&gt;::construct(_Up*, _Args&amp;&amp; ...) [with _Up = g2o::SE3Quat; _Args = {Eigen::AngleAxis&lt;double&gt;&amp;, Eigen::Matrix&lt;double, 3, 1, 0, 3, 1&gt;&amp;}; _Tp = g2o::SE3Quat]&rsquo;: /usr/include/c++/9/bits/alloc_traits.h:483:4: required from &lsquo;static void std::allocator_traits&lt;std::allocator&lt;_CharT&gt; &gt;::construct(std::allocator_traits&lt;std::allocator&lt;_CharT&gt; &gt;::allocator_type&amp;, _Up*, _Args&amp;&amp; ...) [with _Up = g2o::SE3Quat; _Args = {Eigen::AngleAxis&lt;double&gt;&amp;, Eigen::Matrix&lt;double, 3, 1, 0, 3, 1&gt;&amp;}; _Tp = g2o::SE3Quat; std::allocator_traits&lt;std::allocator&lt;_CharT&gt; &gt;::allocator_type = std::allocator&lt;g2o::SE3Quat&gt;]&rsquo; /usr/include/c++/9/bits/vector.tcc:115:30: required from &lsquo;void std::vector&lt;_Tp, _Alloc&gt;::emplace_back(_Args&amp;&amp; ...) [with _Args = {Eigen::AngleAxis&lt;double&gt;&amp;, Eigen::Matrix&lt;double, 3, 1, 0, 3, 1&gt;&amp;}; _Tp = g2o::SE3Quat; _Alloc = std::allocator&lt;g2o::SE3Quat&gt;]&rsquo; /home/guo/g2o仿真/src/optimize.cc:66:54: required from here /usr/include/c++/9/ext/new_allocator.h:146:4: error: no matching function for call to &lsquo;g2o::SE3Quat::SE3Quat(Eigen::AngleAxis&lt;double&gt;&amp;, Eigen::Matrix&lt;double, 3, 1&gt;&amp;)&rsquo; 146 | { ::new((void *)__p) _Up(std::forward&lt;_Args&gt;(__args)...); } | ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
最新发布
08-03
#include &lt;cmath&gt; #include &lt;iostream&gt; #include &lt;Eigen/Dense&gt; #include &lt;Eigen/Geometry&gt; #include &lt;Sophus/SE3.hpp&gt; #include &lt;pcl/io/pcd_io.h&gt; #include &lt;pcl/point_types.h&gt; #include &lt;pcl/point_cloud.h&gt; #include &lt;pcl/common/transforms.h&gt; #include &lt;pcl/kdtree/kdtree_flann.h&gt; #include &lt;pcl/features/normal_3d_omp.h&gt; // 辅助函数:计算向量的反对称矩阵 Eigen::Matrix3f skew(const Eigen::Vector3f&amp; v) { Eigen::Matrix3f mat; mat &lt;&lt; 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return mat; } // 稀疏收缩函数 Eigen::Vector3f shrink(const Eigen::Vector3f&amp; h, double p, double mu) { Eigen::Vector3f result; for (int i = 0; i &lt; 3; ++i) { double abs_h = std::abs(h[i]); if (abs_h &lt;= 1.0 / mu) { result[i] = 0.0; } else { result[i] = std::copysign(abs_h - 1.0 / mu, h[i]) * std::pow(abs_h, p - 1); } } return result; } // 单值收缩函数(用于点对面距离) double shrink(double h, double p, double mu) { double abs_h = std::abs(h); if (abs_h &lt;= 1.0 / mu) { return 0.0; } else { return std::copysign(abs_h - 1.0 / mu, h) * std::pow(abs_h, p - 1); } } // 主ICP函数 Eigen::Matrix4f WES_ICP_Optimization( const pcl::PointCloud&lt;pcl::PointXYZ&gt;&amp; source_cloud, const pcl::PointCloud&lt;pcl::PointXYZ&gt;&amp; inter_cloud, const pcl::PointCloud&lt;pcl::PointXYZ&gt;&amp; target_cloud, const pcl::PointCloud&lt;pcl::Normal&gt;&amp; normals_cloud, const Eigen::VectorXf&amp; d, float w1, float w2) { // 初始化线性系统 Eigen::Matrix&lt;float, 6, 6&gt; A = Eigen::Matrix&lt;float, 6, 6&gt;::Zero(); Eigen::Matrix&lt;float, 6, 1&gt; b = Eigen::Matrix&lt;float, 6, 1&gt;::Zero(); // 遍历每个点 for (size_t i = 0; i &lt; source_cloud.size(); ++i) { // 获取当前点坐标 Eigen::Vector3f source_pt = source_cloud.points[i].getVector3fMap(); Eigen::Vector3f inter_pt = inter_cloud.points[i].getVector3fMap(); Eigen::Vector3f target_pt = target_cloud.points[i].getVector3fMap(); Eigen::Vector3f normal = normals_cloud.points[i].getNormalVector3fMap(); float d_i = d[i]; // 1. 构造生成元矩阵G (4x6) Eigen::Matrix&lt;float, 4, 6&gt; G; G.setZero(); G.block&lt;3, 3&gt;(0, 0) = Eigen::Matrix3f::Identity(); // 平移部分 G.block&lt;3, 3&gt;(0, 3) = -skew(source_pt); // 旋转部分(反对称矩阵) // 2. 计算残差项 // 点对点残差 (e1) Eigen::Vector4f e1; e1.head&lt;3&gt;() = source_pt - inter_pt; e1(3) = 0.0; // 点对平面残差 (e2) float e2 = (source_pt - target_pt).dot(normal) - d_i; // 3. 计算加权雅可比矩阵 Eigen::Matrix&lt;float, 4, 6&gt; A1 = G; // 点对点部分 Eigen::Vector4f normal_hom(normal.x(), normal.y(), normal.z(), 0.0); Eigen::Matrix&lt;float, 1, 6&gt; A2 = normal_hom.transpose() * G; // 点对平面部分 // 4. 更新线性系统 A += w1 * A1.transpose() * A1 + w2 * A2.transpose() * A2; b -= w1 * A1.transpose() * e1 + w2 * A2.transpose() * e2; } // 5. 求解线性系统 (A*x = b) Eigen::Matrix&lt;float, 6, 1&gt; x = A.ldlt().solve(b); // 6. 将李代数转换为SE3变换 Sophus::SE3f T_update = Sophus::SE3f::exp(x); // 7. 转换为Eigen::Matrix4f并返回 return T_update.matrix(); } // 主算法函数 void sparseWeightedDistanceICP( pcl::PointCloud&lt;pcl::PointXYZ&gt;&amp; source_points, pcl::PointCloud&lt;pcl::PointXYZ&gt;&amp; target_points, pcl::PointCloud&lt;pcl::Normal&gt;&amp; target_normals, int max_icp, int max_outer, int max_inner, float p, Eigen::Matrix4f&amp; final_transform) { std::cout &lt;&lt; &quot;WES-ICP (Weighted Euclidean Distance Sparse ICP):&quot; &lt;&lt; std::endl; if (source_points.empty() || target_points.empty()) { return; } // 初始化变量 pcl::PointCloud&lt;pcl::PointXYZ&gt; move_points = source_points; pcl::PointCloud&lt;pcl::PointXYZ&gt; old_points = source_points; // 乘数交替方向法(ADMM)变量 const size_t num_points = source_points.size(); Eigen::MatrixXf lambda_pTp = Eigen::MatrixXf::Zero(3, num_points); Eigen::MatrixXf Z_pTp = Eigen::MatrixXf::Zero(3, num_points); Eigen::VectorXf lambda_pTpln = Eigen::VectorXf::Zero(num_points); Eigen::VectorXf Z_pTpln = Eigen::VectorXf::Zero(num_points); Eigen::VectorXf delta_pTpln = Eigen::VectorXf::Zero(num_points); float mu = 10.0; final_transform = Eigen::Matrix4f::Identity(); // 构建KD树 pcl::KdTreeFLANN&lt;pcl::PointXYZ&gt; kdtree; kdtree.setInputCloud(target_points.makeShared()); for (int icp = 0; icp &lt; max_icp; ++icp) { std::vector&lt;int&gt; indices(1); // 只需要最近1个邻点 std::vector&lt;float&gt; distances(1); // 获取匹配点和法线 pcl::PointCloud&lt;pcl::PointXYZ&gt; match_points; pcl::PointCloud&lt;pcl::Normal&gt; match_normals; for (size_t i = 0; i &lt; move_points.size(); ++i) { // 对每个点单独搜索 if (kdtree.nearestKSearch(move_points[i], 1, indices, distances) &lt;= 0) { std::cerr &lt;&lt; &quot;Warning: No neighbor found for point &quot; &lt;&lt; i &lt;&lt; std::endl; continue; } // 使用搜索结果 int nearest_idx = indices[0]; float nearest_dist = distances[0]; match_points.push_back(target_points[nearest_idx]); match_normals.push_back(target_normals[nearest_idx]); } // 计算权重(随迭代动态调整) float w_pTp = 2.0 / (1.0 + std::exp(max_icp - icp)); float w_pTpln = 1.0 - w_pTp; std::cout &lt;&lt; &quot;ICP iteration &quot; &lt;&lt; icp + 1 &lt;&lt; &quot;/&quot; &lt;&lt; max_icp &lt;&lt; &quot; (weights: pTp=&quot; &lt;&lt; w_pTp &lt;&lt; &quot;, pTpln=&quot; &lt;&lt; w_pTpln &lt;&lt; &quot;)&quot; &lt;&lt; std::endl; // 2. ADMM外层循环 for (int outer = 0; outer &lt; max_outer; ++outer) { // 3. ADMM内层循环 for (int inner = 0; inner &lt; max_inner; ++inner) { // 点对点稀疏收缩 Eigen::MatrixXf H_pTp = move_points.getMatrixXfMap(3, 4, 0).transpose() - match_points.getMatrixXfMap(3, 4, 0).transpose() + lambda_pTp / mu; for (size_t k = 0; k &lt; num_points; ++k) { Z_pTp.col(k) = shrink(H_pTp.col(k), p, mu); } pcl::PointCloud&lt;pcl::PointXYZ&gt; inter_points; for (size_t k = 0; k &lt; num_points; ++k) { Eigen::Vector3f pt = match_points[k].getVector3fMap() + Z_pTp.col(k) - lambda_pTp.col(k) / mu; inter_points.push_back(pcl::PointXYZ(pt.x(), pt.y(), pt.z())); } // 点对面稀疏收缩 Eigen::VectorXf H_pTpln(num_points); for (size_t k = 0; k &lt; num_points; ++k) { H_pTpln(k) = match_normals[k].getNormalVector3fMap().dot( move_points[k].getVector3fMap() - match_points[k].getVector3fMap()); } H_pTpln += lambda_pTpln / mu; for (size_t k = 0; k &lt; num_points; ++k) { Z_pTpln(k) = shrink(H_pTpln(k), p, mu); } Eigen::VectorXf inter_d = Z_pTpln - lambda_pTpln / mu; // 求解变换矩阵 Eigen::Matrix4f T1 = WES_ICP_Optimization( move_points, inter_points, match_points, match_normals, inter_d, w_pTp, w_pTpln); // 更新变换和点云 final_transform = T1 * final_transform; pcl::transformPointCloud(move_points, move_points, T1); // 检查对偶残差 float dual_max = 0.0; for (size_t k = 0; k &lt; num_points; ++k) { Eigen::Vector3f diff = old_points[k].getVector3fMap() - move_points[k].getVector3fMap(); float dual_pTpln = diff.dot(match_normals[k].getNormalVector3fMap()); float dual_pTp = diff.norm(); dual_max = std::max(dual_max, std::max(dual_pTpln, dual_pTp)); } old_points = move_points; if (dual_max &lt; 1e-5) { break; } } // 计算原始残差 float prime_max = 0.0; // 点对点残差 Eigen::MatrixXf delta_pTp = move_points.getMatrixXfMap(3, 4, 0).transpose() - match_points.getMatrixXfMap(3, 4, 0).transpose() - Z_pTp; // 点对面残差 for (size_t k = 0; k &lt; num_points; ++k) { delta_pTpln(k) = match_normals[k].getNormalVector3fMap().dot( move_points[k].getVector3fMap() - match_points[k].getVector3fMap()) - Z_pTpln(k); float prime_pTpln = std::abs(delta_pTpln(k)); float prime_pTp = delta_pTp.col(k).norm(); prime_max = std::max(prime_max, std::max(prime_pTpln, prime_pTp)); } // 更新拉格朗日乘子 lambda_pTpln += mu * delta_pTpln; lambda_pTp += mu * delta_pTp; // 检查收敛条件 float dual_max = 0.0; for (size_t k = 0; k &lt; num_points; ++k) { Eigen::Vector3f diff = old_points[k].getVector3fMap() - move_points[k].getVector3fMap(); float dual_pTpln = diff.dot(match_normals[k].getNormalVector3fMap()); float dual_pTp = diff.norm(); dual_max = std::max(dual_max, std::max(dual_pTpln, dual_pTp)); } if (dual_max &lt; 1e-5 &amp;&amp; prime_max &lt; 1e-5) { break; } } } } int main() { // 加载点云 pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr source(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::io::loadPCDFile&lt;pcl::PointXYZ&gt;(&quot;A//source//13sourceGridCloud2.pcd&quot;, *source); pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr target(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::io::loadPCDFile&lt;pcl::PointXYZ&gt;(&quot;A//target//13targetGridCloud2.pcd&quot;, *target); pcl::PointCloud&lt;pcl::Normal&gt;::Ptr target_normals(new pcl::PointCloud&lt;pcl::Normal&gt;); pcl::NormalEstimationOMP&lt;pcl::PointXYZ, pcl::Normal&gt; n; pcl::search::KdTree&lt;pcl::PointXYZ&gt;::Ptr tree(new pcl::search::KdTree&lt;pcl::PointXYZ&gt;); n.setNumberOfThreads(4); n.setInputCloud(target); n.setSearchMethod(tree); n.setKSearch(20); n.compute(*target_normals); // 设置参数 int max_icp = 50; // ICP最大迭代次数 int max_outer = 10; // ADMM外层循环次数 int max_inner = 5; // ADMM内层循环次数 double p = 2.5; // p-norm参数 // 执行配准 Eigen::Matrix4f transformation; sparseWeightedDistanceICP(*source, *target, *target_normals, max_icp, max_outer, max_inner, p, transformation); // 应用变换到源点云 pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr out_cloud(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::transformPointCloud(*source, *out_cloud, transformation); pcl::io::savePCDFileBinary(&quot;A//0sourceWESICP.pcd&quot;, *out_cloud); return 0; } 封装成类,.h和.cpp分开保存,要求输入为待配准点云,参数设置,输出为配准变换矩阵
07-02
#include &lt;iostream&gt; #include &lt;vector&gt; #include &lt;Eigen/Dense&gt; #include &lt;pcl/io/pcd_io.h&gt; #include &lt;pcl/point_cloud.h&gt; #include &lt;pcl/point_types.h&gt; #include &lt;pcl/kdtree/kdtree_flann.h&gt; #include &lt;pcl/common/transforms.h&gt; #include &lt;pcl/registration/transformation_estimation_svd.h&gt; //svd // 稀疏收缩函数(向量形式) Eigen::Vector3f shrink(const Eigen::Vector3f&amp; h, float p, float mu) { Eigen::Vector3f result; for (int i = 0; i &lt; 3; ++i) { float abs_h = std::abs(h[i]); if (abs_h &lt;= 1.0f / mu) { result[i] = 0.0f; } else { result[i] = (h[i] &gt; 0 ? 1 : -1) * (abs_h - 1.0f / mu) * std::pow(abs_h, p - 1); } } return result; } // 主函数 void SparsePointToPoint(pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr&amp; move_points, Eigen::Matrix4f&amp; T, const pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr&amp; source_points, const pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr&amp; target_points, int max_icp, int max_outer, int max_inner, float p) { std::cout &lt;&lt; &quot;sparse point-to-point:&quot; &lt;&lt; std::endl; // 初始化 *move_points = *source_points; pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr old_points(new pcl::PointCloud&lt;pcl::PointXYZ&gt;(*source_points)); // 构建KD树 pcl::KdTreeFLANN&lt;pcl::PointXYZ&gt; kdtree; kdtree.setInputCloud(target_points); // ADMM变量 const size_t Num = source_points-&gt;size(); Eigen::MatrixXf lambda = Eigen::MatrixXf::Zero(3, Num); Eigen::MatrixXf Z = Eigen::MatrixXf::Zero(3, Num); float mu = 10.0f; T = Eigen::Matrix4f::Identity(); // 主循环 for (int icp = 1; icp &lt;= max_icp; ++icp) { // 匹配点 std::vector&lt;int&gt; indices(1); std::vector&lt;float&gt; distances(1); pcl::PointCloud&lt;pcl::PointXYZ&gt; match_points; for (size_t i = 0; i &lt; move_points-&gt;size(); ++i) { if (kdtree.nearestKSearch(move_points-&gt;points[i], 1, indices, distances) &lt;= 0) { continue; } int nearest_idx = indices[0]; match_points.push_back(target_points-&gt;points[nearest_idx]); } std::cout &lt;&lt; &quot;iteration at &quot; &lt;&lt; icp &lt;&lt; &quot;-&quot; &lt;&lt; max_icp &lt;&lt; std::endl; // ADMM外层循环 for (int outer = 1; outer &lt;= max_outer; ++outer) { // ADMM内层循环 for (int inner = 1; inner &lt;= max_inner; ++inner) { // 稀疏收缩 Eigen::MatrixXf H = move_points-&gt;getMatrixXfMap(3, 4, 0).transpose() - match_points.getMatrixXfMap(3, 4, 0).transpose() + lambda / mu; for (size_t k = 0; k &lt; Num; ++k) { Z.col(k) = shrink(H.col(k), p, mu); } // 计算变换 pcl::PointCloud&lt;pcl::PointXYZ&gt; C; for (size_t k = 0; k &lt; Num; ++k) { Eigen::Vector3f pt = match_points[k].getVector3fMap() + Z.col(k) - lambda.col(k) / mu; C.push_back(pcl::PointXYZ(pt.x(), pt.y(), pt.z())); } // 利用SVD方法求解变换矩阵 pcl::registration::TransformationEstimationSVD&lt;pcl::PointXYZ, pcl::PointXYZ&gt; TESVD; Eigen::Matrix4f T1 = Eigen::Matrix4f::Identity(); TESVD.estimateRigidTransformation(*move_points, C, T1); // 更新变换矩阵 T = T1 * T; // 应用变换 pcl::transformPointCloud(*move_points, *move_points, T1); // 检查收敛 float dual_max = 0.0f; for (size_t m = 0; m &lt; Num; ++m) { float dual = (old_points-&gt;points[m].getVector3fMap() - move_points-&gt;points[m].getVector3fMap()).norm(); dual_max = std::max(dual_max, dual); } *old_points = *move_points; if (dual_max &lt; 1e-5f) { break; } } // 更新拉格朗日乘子 Eigen::MatrixXf delta = move_points-&gt;getMatrixXfMap(3, 4, 0).transpose() - match_points.getMatrixXfMap(3, 4, 0).transpose() - Z; float prime_max = 0.0f; for (size_t k = 0; k &lt; Num; ++k) { float prime = delta.col(k).norm(); prime_max = std::max(prime_max, prime); } lambda += mu * delta; // 检查收敛 float dual_max = 0.0f; for (size_t m = 0; m &lt; Num; ++m) { float dual = (old_points-&gt;points[m].getVector3fMap() - move_points-&gt;points[m].getVector3fMap()).norm(); dual_max = std::max(dual_max, dual); } if (dual_max &lt; 1e-5f &amp;&amp; prime_max &lt; 1e-5f) { break; } } } } int main() { // 加载点云 pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr source(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::io::loadPCDFile&lt;pcl::PointXYZ&gt;(&quot;A//coarse_source_points.pcd&quot;, *source); pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr target(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::io::loadPCDFile&lt;pcl::PointXYZ&gt;(&quot;A//target_points.pcd&quot;, *target); // 参数设置 int max_icp = 50; int max_outer = 10; int max_inner = 5; float p = 2.5f; // 执行配准 pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr move_points(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); Eigen::Matrix4f transformation; SparsePointToPoint(move_points, transformation, source, target, max_icp, max_outer, max_inner, p); // 输出结果 std::cout &lt;&lt; &quot;Transformation Matrix:\n&quot; &lt;&lt; transformation &lt;&lt; std::endl; // 应用变换到源点云 pcl::PointCloud&lt;pcl::PointXYZ&gt;::Ptr out_cloud(new pcl::PointCloud&lt;pcl::PointXYZ&gt;); pcl::transformPointCloud(*source, *out_cloud, transformation); pcl::io::savePCDFileBinary(&quot;A//sourceSICP.pcd&quot;, *out_cloud); return 0; } 封装成类,在适当位置添加OMP多线程加速,.h和.cpp分开保存,要求输入为待配准点云,参数设置,输出为变换矩阵。
07-02
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值