From 6c22ff9bf88c537b0ce40518a22a3c8e571ff22a Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 3 Mar 2024 17:09:56 +0100 Subject: [PATCH] Optimize Eigen block operations According to https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html , Eigen should receive as much information as possible at compile time, to generate optimal machine code. This means specifying the block size as a template parameter (if fixed size), using topLeftCorner if block starts at (0, 0) --- .../filesystem_face_detection.cpp | 4 ++-- common/include/pcl/common/impl/centroid.hpp | 8 ++++---- common/include/pcl/common/impl/transforms.hpp | 2 +- .../pcl/people/impl/head_based_subcluster.hpp | 4 ++-- .../include/pcl/people/impl/height_map_2d.hpp | 8 ++++---- .../face_detector_data_provider.cpp | 4 ++-- .../rf_face_detector_trainer.cpp | 4 ++-- .../include/pcl/registration/impl/elch.hpp | 4 ++-- .../include/pcl/registration/impl/gicp.hpp | 8 ++++---- .../include/pcl/registration/impl/ia_fpcs.hpp | 2 +- .../include/pcl/registration/impl/ia_kfpcs.hpp | 6 +++--- .../include/pcl/registration/impl/lum.hpp | 4 ++-- .../impl/transformation_estimation_2D.hpp | 9 +++++---- .../impl/transformation_estimation_3point.hpp | 18 +++++++++--------- .../impl/transformation_estimation_svd.hpp | 9 +++++---- .../transformation_estimation_svd_scale.hpp | 11 ++++++----- .../pcl/registration/warp_point_rigid.h | 4 ++-- .../pcl/registration/warp_point_rigid_3d.h | 4 ++-- .../pcl/registration/warp_point_rigid_6d.h | 2 +- registration/src/gicp6d.cpp | 8 ++------ test/filters/test_filters.cpp | 4 ++-- tools/transform_point_cloud.cpp | 4 ++-- .../pcl/visualization/impl/pcl_visualizer.hpp | 4 ++-- visualization/src/interactor_style.cpp | 2 +- visualization/src/pcl_visualizer.cpp | 4 ++-- 25 files changed, 70 insertions(+), 71 deletions(-) diff --git a/apps/src/face_detection/filesystem_face_detection.cpp b/apps/src/face_detection/filesystem_face_detection.cpp index f69b8f94f73..a13a49c752f 100644 --- a/apps/src/face_detection/filesystem_face_detection.cpp +++ b/apps/src/face_detection/filesystem_face_detection.cpp @@ -98,7 +98,7 @@ run(pcl::RFFaceDetectorTrainer& fdrf, bool result = face_detection_apps_utils::readMatrixFromFile(pose_file, pose_mat); if (result) { - Eigen::Vector3f ea = pose_mat.block<3, 3>(0, 0).eulerAngles(0, 1, 2); + Eigen::Vector3f ea = pose_mat.topLeftCorner<3, 3>().eulerAngles(0, 1, 2); Eigen::Vector3f trans_vector = Eigen::Vector3f(pose_mat(0, 3), pose_mat(1, 3), pose_mat(2, 3)); std::cout << ea << std::endl; @@ -127,7 +127,7 @@ run(pcl::RFFaceDetectorTrainer& fdrf, Eigen::AngleAxisf(ea[1], Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(ea[2], Eigen::Vector3f::UnitZ()); - // matrixxx = pose_mat.block<3,3>(0,0); + // matrixxx = pose_mat.topLeftCorner<3,3>(); vec = matrixxx * vec; cylinder_coeff.values[3] = vec[0]; diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp index 30697e79e3a..bc0885be58f 100644 --- a/common/include/pcl/common/impl/centroid.hpp +++ b/common/include/pcl/common/impl/centroid.hpp @@ -695,8 +695,8 @@ computeCentroidAndOBB (const pcl::PointCloud &cloud, //[R^t , -R^t*Centroid ] //[0 , 1 ] Eigen::Matrix transform = Eigen::Matrix::Identity(); - transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose(); - transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid; + transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose(); + transform.template topRightCorner<3, 1>() =-transform.template topLeftCorner<3, 3>()*centroid; //when Scalar==double on a Windows 10 machine and MSVS: //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse @@ -829,8 +829,8 @@ computeCentroidAndOBB (const pcl::PointCloud &cloud, //[R^t , -R^t*Centroid ] //[0 , 1 ] Eigen::Matrix transform = Eigen::Matrix::Identity(); - transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose(); - transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid; + transform.template topLeftCorner<3, 3>() = obb_rotational_matrix.transpose(); + transform.template topRightCorner<3, 1>() =-transform.template topLeftCorner<3, 3>()*centroid; //when Scalar==double on a Windows 10 machine and MSVS: //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp index 6c884d16c08..c255161b51e 100644 --- a/common/include/pcl/common/impl/transforms.hpp +++ b/common/include/pcl/common/impl/transforms.hpp @@ -507,7 +507,7 @@ getPrincipalTransformation (const pcl::PointCloud &cloud, double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1); double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2); - transform.translation () = centroid.head (3); + transform.translation () = centroid.template head<3> (); transform.linear () = eigen_vects; return (std::min (rel1, rel2)); diff --git a/people/include/pcl/people/impl/head_based_subcluster.hpp b/people/include/pcl/people/impl/head_based_subcluster.hpp index 7c9866fcf27..950f56854de 100644 --- a/people/include/pcl/people/impl/head_based_subcluster.hpp +++ b/people/include/pcl/people/impl/head_based_subcluster.hpp @@ -136,7 +136,7 @@ pcl::people::HeadBasedSubclustering::mergeClustersCloseInFloorCoordinate { float min_distance_between_cluster_centers = 0.4; // meters float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed) - Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed) + Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head<3>(); // ground plane normal (precomputed for speed) std::vector > connected_clusters; connected_clusters.resize(input_clusters.size()); std::vector used_clusters; // 0 in correspondence of clusters remained to process, 1 for already used clusters @@ -196,7 +196,7 @@ pcl::people::HeadBasedSubclustering::createSubClusters (pcl::people::Per { // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices: float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed) - Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed) + Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head<3>(); // ground plane normal (precomputed for speed) Eigen::Matrix3Xf maxima_projected(3,maxima_number); // matrix containing the projection of maxima onto the ground plane Eigen::VectorXi subclusters_number_of_points(maxima_number); // subclusters number of points std::vector sub_clusters_indices; // vector of vectors with the cluster indices for every maximum diff --git a/people/include/pcl/people/impl/height_map_2d.hpp b/people/include/pcl/people/impl/height_map_2d.hpp index e8640ebd768..df93b822c60 100644 --- a/people/include/pcl/people/impl/height_map_2d.hpp +++ b/people/include/pcl/people/impl/height_map_2d.hpp @@ -213,16 +213,16 @@ pcl::people::HeightMap2D::filterMaxima () PointT* p_current = &(*cloud_)[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen - float t = p_current_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground - p_current_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane + float t = p_current_eigen.dot(ground_coeffs_.head<3>()) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground + p_current_eigen -= ground_coeffs_.head<3>() * t; // projection of the point on the groundplane int j = i-1; while ((j >= 0) && (good_maximum)) { PointT* p_previous = &(*cloud_)[maxima_cloud_indices_[j]]; // pointcloud point referring to an already validated maximum Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z); // conversion to eigen - float t = p_previous_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground - p_previous_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane + float t = p_previous_eigen.dot(ground_coeffs_.head<3>()) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground + p_previous_eigen -= ground_coeffs_.head<3>() * t; // projection of the point on the groundplane // distance of the projection of the points on the groundplane: float distance = (p_current_eigen-p_previous_eigen).norm(); diff --git a/recognition/src/face_detection/face_detector_data_provider.cpp b/recognition/src/face_detection/face_detector_data_provider.cpp index 558c7f10773..f2b7ab62520 100644 --- a/recognition/src/face_detection/face_detector_data_provider.cpp +++ b/recognition/src/face_detection/face_detector_data_provider.cpp @@ -93,7 +93,7 @@ void pcl::face_detection::FaceDetectorDataProvider (0, 0).eulerAngles (0, 1, 2); + Eigen::Vector3f ea = pose_mat.topLeftCorner<3, 3> ().eulerAngles (0, 1, 2); ea *= 57.2957795f; //transform it to degrees to do the binning int y = static_cast(pcl_round ((ea[0] + static_cast(std::abs (min_yaw))) / res_yaw)); int p = static_cast(pcl_round ((ea[1] + static_cast(std::abs (min_pitch))) / res_pitch)); @@ -354,7 +354,7 @@ void pcl::face_detection::FaceDetectorDataProvider (0, 0).eulerAngles (0, 1, 2); + Eigen::Vector3f ea = pose_mat.topLeftCorner<3, 3> ().eulerAngles (0, 1, 2); Eigen::Vector3f trans_vector = Eigen::Vector3f (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3)); pcl::PointXYZ center_point; diff --git a/recognition/src/face_detection/rf_face_detector_trainer.cpp b/recognition/src/face_detection/rf_face_detector_trainer.cpp index e4a447b4d81..327c132c47a 100644 --- a/recognition/src/face_detection/rf_face_detector_trainer.cpp +++ b/recognition/src/face_detection/rf_face_detector_trainer.cpp @@ -479,7 +479,7 @@ void pcl::RFFaceDetectorTrainer::detectFaces() Eigen::Matrix4f guess; guess.setIdentity (); - guess.block<3, 3> (0, 0) = matrixxx; + guess.topLeftCorner<3, 3> () = matrixxx; guess (0, 3) = head_clusters_centers_[i][0]; guess (1, 3) = head_clusters_centers_[i][1]; guess (2, 3) = head_clusters_centers_[i][2]; @@ -519,7 +519,7 @@ void pcl::RFFaceDetectorTrainer::detectFaces() head_clusters_centers_[i][1] = icp_trans (1, 3); head_clusters_centers_[i][2] = icp_trans (2, 3); - Eigen::Vector3f ea = icp_trans.block<3, 3> (0, 0).eulerAngles (0, 1, 2); + Eigen::Vector3f ea = icp_trans.topLeftCorner<3, 3> ().eulerAngles (0, 1, 2); head_clusters_rotation_[i][0] = ea[0]; head_clusters_rotation_[i][1] = ea[1]; head_clusters_rotation_[i][2] = ea[2]; diff --git a/registration/include/pcl/registration/impl/elch.hpp b/registration/include/pcl/registration/impl/elch.hpp index 647386cf880..b1dc7535c53 100644 --- a/registration/include/pcl/registration/impl/elch.hpp +++ b/registration/include/pcl/registration/impl/elch.hpp @@ -192,7 +192,7 @@ pcl::registration::ELCH::initCompute() PointCloudPtr tmp(new PointCloud); // Eigen::Vector4f diff = pose_start - pose_end; - // Eigen::Translation3f translation (diff.head (3)); + // Eigen::Translation3f translation (diff.head<3> ()); // Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity (); // pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans); @@ -240,7 +240,7 @@ pcl::registration::ELCH::compute() // TODO use pose // Eigen::Vector4f cend; // pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend); - // Eigen::Translation3f tend (cend.head (3)); + // Eigen::Translation3f tend (cend.head<3> ()); // Eigen::Affine3f aend (tend); // Eigen::Affine3f aendI = aend.inverse (); diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index 3e1bcb62ed6..f2101ad3a45 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -635,9 +635,9 @@ GeneralizedIterativeClosestPoint:: p_trans_src[1] - p_tgt[1], p_trans_src[2] - p_tgt[2]); const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx); - const Eigen::Vector3d Md(M * d); // Md = M*d - gradient.head<3>() += Md; // translation gradient - hessian.block<3, 3>(0, 0) += M; // translation-translation hessian + const Eigen::Vector3d Md(M * d); // Md = M*d + gradient.head<3>() += Md; // translation gradient + hessian.topLeftCorner<3, 3>() += M; // translation-translation hessian p_trans_src = base_transformation_float * p_src; const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]); dCost_dR_T.noalias() += p_base_src * Md.transpose(); @@ -657,7 +657,7 @@ GeneralizedIterativeClosestPoint:: gradient.head<3>() *= 2.0 / m; // translation gradient dCost_dR_T *= 2.0 / m; gicp_->computeRDerivative(x, dCost_dR_T, gradient); // rotation gradient - hessian.block<3, 3>(0, 0) *= 2.0 / m; // translation-translation hessian + hessian.topLeftCorner<3, 3>() *= 2.0 / m; // translation-translation hessian // translation-rotation hessian dCost_dR_T1.row(0) = dCost_dR_T1b.col(0); dCost_dR_T1.row(1) = dCost_dR_T2b.col(0); diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp index 4521bb0c0d2..b3445f22d64 100644 --- a/registration/include/pcl/registration/impl/ia_fpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp @@ -350,7 +350,7 @@ pcl::registration::FPCSInitialAlignmentgetVector3fMap() - centre_pt.head(3)).squaredNorm(); + float d4 = (pt4->getVector3fMap() - centre_pt.head<3>()).squaredNorm(); // check distance between points w.r.t minimum sampling distance; EDITED -> 4th // point now also limited by max base line diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp index 68237fb7059..b42ae57e32f 100644 --- a/registration/include/pcl/registration/impl/ia_kfpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_kfpcs.hpp @@ -166,7 +166,7 @@ KFPCSInitialAlignment:: // translation score (solutions with small translation are down-voted) float scale = 1.f; if (use_trl_score_) { - float trl = transformation.rightCols<1>().head(3).norm(); + float trl = transformation.rightCols<1>().head<3>().norm(); float trl_ratio = (trl - lower_trl_boundary_) / (upper_trl_boundary_ - lower_trl_boundary_); @@ -244,7 +244,7 @@ KFPCSInitialAlignment::getNBestCandid for (const auto& c2 : candidates) { Eigen::Matrix4f diff = candidate.transformation.colPivHouseholderQr().solve(c2.transformation); - const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle(); + const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle(); const float translation3d = diff.block<3, 1>(0, 3).norm(); unique = angle3d > min_angle3d && translation3d > min_translation3d; if (!unique) { @@ -281,7 +281,7 @@ KFPCSInitialAlignment::getTBestCandid for (const auto& c2 : candidates) { Eigen::Matrix4f diff = candidate.transformation.colPivHouseholderQr().solve(c2.transformation); - const float angle3d = Eigen::AngleAxisf(diff.block<3, 3>(0, 0)).angle(); + const float angle3d = Eigen::AngleAxisf(diff.topLeftCorner<3, 3>()).angle(); const float translation3d = diff.block<3, 1>(0, 3).norm(); unique = angle3d > min_angle3d && translation3d > min_translation3d; if (!unique) { diff --git a/registration/include/pcl/registration/impl/lum.hpp b/registration/include/pcl/registration/impl/lum.hpp index faa1d6f575d..534c4951684 100644 --- a/registration/include/pcl/registration/impl/lum.hpp +++ b/registration/include/pcl/registration/impl/lum.hpp @@ -254,8 +254,8 @@ LUM::compute() // Fill in elements of G and B if (vj > 0) - G.block(6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_; - G.block(6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_; + G.block<6, 6>(6 * (vi - 1), 6 * (vj - 1)) = -(*slam_graph_)[e].cinv_; + G.block<6, 6>(6 * (vi - 1), 6 * (vi - 1)) += (*slam_graph_)[e].cinv_; B.segment(6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_; } } diff --git a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp index c2aae92c166..1140880b547 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp @@ -166,7 +166,7 @@ TransformationEstimation2D:: // Assemble the correlation matrix H = source * target' Eigen::Matrix H = - (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3); + (cloud_src_demean * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>(); float angle = std::atan2((H(0, 1) - H(1, 0)), (H(0, 0) + H(1, 1))); @@ -176,9 +176,10 @@ TransformationEstimation2D:: R(1, 0) = std::sin(angle); // Return the correct transformation - transformation_matrix.topLeftCorner(3, 3).matrix() = R; - const Eigen::Matrix Rc(R * centroid_src.head(3).matrix()); - transformation_matrix.block(0, 3, 3, 1).matrix() = centroid_tgt.head(3) - Rc; + transformation_matrix.template topLeftCorner<3, 3>().matrix() = R; + const Eigen::Matrix Rc(R * centroid_src.template head<3>().matrix()); + transformation_matrix.template block<3, 1>(0, 3).matrix() = + centroid_tgt.template head<3>() - Rc; } } // namespace registration diff --git a/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp index cceac5530db..c4cf1671aee 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_3point.hpp @@ -155,11 +155,11 @@ pcl::registration::TransformationEstimation3Point s1 = - source_demean.col(1).head(3) - source_demean.col(0).head(3); + source_demean.col(1).template head<3>() - source_demean.col(0).template head<3>(); s1.normalize(); Eigen::Matrix s2 = - source_demean.col(2).head(3) - source_demean.col(0).head(3); + source_demean.col(2).template head<3>() - source_demean.col(0).template head<3>(); s2 -= s2.dot(s1) * s1; s2.normalize(); @@ -169,11 +169,11 @@ pcl::registration::TransformationEstimation3Point t1 = - target_demean.col(1).head(3) - target_demean.col(0).head(3); + target_demean.col(1).template head<3>() - target_demean.col(0).template head<3>(); t1.normalize(); Eigen::Matrix t2 = - target_demean.col(2).head(3) - target_demean.col(0).head(3); + target_demean.col(2).template head<3>() - target_demean.col(0).template head<3>(); t2 -= t2.dot(t1) * t1; t2.normalize(); @@ -184,11 +184,11 @@ pcl::registration::TransformationEstimation3Point R = source_rot * target_rot.transpose (); Eigen::Matrix R = target_rot * source_rot.transpose(); - transformation_matrix.topLeftCorner(3, 3) = R; - // transformation_matrix.block (0, 3, 3, 1) = source_mean.head (3) - R * - // target_mean.head (3); - transformation_matrix.block(0, 3, 3, 1) = - target_mean.head(3) - R * source_mean.head(3); + transformation_matrix.template topLeftCorner<3, 3>() = R; + // transformation_matrix.block<3, 1>(0, 3) = source_mean.head<3>() - R * + // target_mean.head<3>(); + transformation_matrix.template block<3, 1>(0, 3) = + target_mean.template head<3>() - R * source_mean.template head<3>(); } //#define PCL_INSTANTIATE_TransformationEstimation3Point(T,U) template class PCL_EXPORTS diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp index 6e62bd92f4f..8ebb59b5770 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp @@ -194,7 +194,7 @@ TransformationEstimationSVD:: // Assemble the correlation matrix H = source * target' Eigen::Matrix H = - (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3); + (cloud_src_demean * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD> svd( @@ -211,9 +211,10 @@ TransformationEstimationSVD:: Eigen::Matrix R = v * u.transpose(); // Return the correct transformation - transformation_matrix.topLeftCorner(3, 3) = R; - const Eigen::Matrix Rc(R * centroid_src.head(3)); - transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc; + transformation_matrix.template topLeftCorner<3, 3>() = R; + const Eigen::Matrix Rc(R * centroid_src.template head<3>()); + transformation_matrix.template block<3, 1>(0, 3) = + centroid_tgt.template head<3>() - Rc; if (pcl::console::isVerbosityLevelEnabled(pcl::console::L_DEBUG)) { size_t N = cloud_src_demean.cols(); diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp index 51a5b2174f7..9b2035af77e 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp @@ -58,7 +58,7 @@ TransformationEstimationSVDScale:: // Assemble the correlation matrix H = source * target' Eigen::Matrix H = - (cloud_src_demean * cloud_tgt_demean.transpose()).topLeftCorner(3, 3); + (cloud_src_demean * cloud_tgt_demean.transpose()).template topLeftCorner<3, 3>(); // Compute the Singular Value Decomposition Eigen::JacobiSVD> svd( @@ -76,7 +76,7 @@ TransformationEstimationSVDScale:: // rotated cloud Eigen::Matrix R4; - R4.block(0, 0, 3, 3) = R; + R4.template topLeftCorner<3, 3>() = R; R4(0, 3) = 0; R4(1, 3) = 0; R4(2, 3) = 0; @@ -96,9 +96,10 @@ TransformationEstimationSVDScale:: } float scale = sum_tt / sum_ss; - transformation_matrix.topLeftCorner(3, 3) = scale * R; - const Eigen::Matrix Rc(scale * R * centroid_src.head(3)); - transformation_matrix.block(0, 3, 3, 1) = centroid_tgt.head(3) - Rc; + transformation_matrix.template topLeftCorner<3, 3>() = scale * R; + const Eigen::Matrix Rc(scale * R * centroid_src.template head<3>()); + transformation_matrix.template block<3, 1>(0, 3) = + centroid_tgt.template head<3>() - Rc; } } // namespace registration diff --git a/registration/include/pcl/registration/warp_point_rigid.h b/registration/include/pcl/registration/warp_point_rigid.h index 2aa6dec1603..228ea9c9092 100644 --- a/registration/include/pcl/registration/warp_point_rigid.h +++ b/registration/include/pcl/registration/warp_point_rigid.h @@ -95,9 +95,9 @@ class WarpPointRigid { pnt_out.z = static_cast( transform_matrix_(2, 0) * pnt_in.x + transform_matrix_(2, 1) * pnt_in.y + transform_matrix_(2, 2) * pnt_in.z + transform_matrix_(2, 3)); - // pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner (3, 3) * + // pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner<3, 3> () * // pnt_in.getVector3fMap () + - // transform_matrix_.block (0, 3, 3, 1); + // transform_matrix_.block<3, 1> (0, 3); // pnt_out.data[3] = pnt_in.data[3]; } diff --git a/registration/include/pcl/registration/warp_point_rigid_3d.h b/registration/include/pcl/registration/warp_point_rigid_3d.h index 459681ccae2..03f2cae9188 100644 --- a/registration/include/pcl/registration/warp_point_rigid_3d.h +++ b/registration/include/pcl/registration/warp_point_rigid_3d.h @@ -82,11 +82,11 @@ class WarpPointRigid3D : public WarpPointRigid(p[0], p[1], 0, 1.0); + trans.template block<4, 1>(0, 3) = Eigen::Matrix(p[0], p[1], 0, 1.0); // Compute w from the unit quaternion Eigen::Rotation2D r(p[2]); - trans.topLeftCorner(2, 2) = r.toRotationMatrix(); + trans.template topLeftCorner<2, 2>() = r.toRotationMatrix(); } }; } // namespace registration diff --git a/registration/include/pcl/registration/warp_point_rigid_6d.h b/registration/include/pcl/registration/warp_point_rigid_6d.h index 30b07d7c216..ec1302acf1a 100644 --- a/registration/include/pcl/registration/warp_point_rigid_6d.h +++ b/registration/include/pcl/registration/warp_point_rigid_6d.h @@ -89,7 +89,7 @@ class WarpPointRigid6D : public WarpPointRigid q(0, p[3], p[4], p[5]); q.w() = static_cast(std::sqrt(1 - q.dot(q))); q.normalize(); - transform_matrix_.topLeftCorner(3, 3) = q.toRotationMatrix(); + transform_matrix_.template topLeftCorner<3, 3>() = q.toRotationMatrix(); } }; } // namespace registration diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp index c06bc2c2664..513c4ef796f 100644 --- a/registration/src/gicp6d.cpp +++ b/registration/src/gicp6d.cpp @@ -225,12 +225,8 @@ GeneralizedIterativeClosestPoint6D::computeTransformation(PointCloudSource& outp PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n", getClassName().c_str()); } - // for some reason the static equivalent method raises an error - // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * - // (guess.block<3,3> (0,0)); final_transformation_.block <3, 1> (0, 3) = - // transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3); - final_transformation_.topLeftCorner(3, 3) = - previous_transformation_.topLeftCorner(3, 3) * guess.topLeftCorner(3, 3); + final_transformation_.topLeftCorner<3, 3>() = + previous_transformation_.topLeftCorner<3, 3>() * guess.topLeftCorner<3, 3>(); final_transformation_(0, 3) = previous_transformation_(0, 3) + guess(0, 3); final_transformation_(1, 3) = previous_transformation_(1, 3) + guess(1, 3); final_transformation_(2, 3) = previous_transformation_(2, 3) + guess(2, 3); diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index aa5f0408651..15e678ab062 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1998,11 +1998,11 @@ TEST (FrustumCulling, Filters) Eigen::AngleAxisf (0 * M_PI / 180, Eigen::Vector3f::UnitY ()) * Eigen::AngleAxisf (0 * M_PI / 180, Eigen::Vector3f::UnitZ ()); - camera_pose.block (0, 0, 3, 3) = R; + camera_pose.topLeftCorner<3, 3> () = R; Eigen::Vector3f T; T (0) = -5; T (1) = 0; T (2) = 0; - camera_pose.block (0, 3, 3, 1) = T; + camera_pose.block<3, 1> (0, 3) = T; camera_pose (3, 3) = 1; fc.setCameraPose (camera_pose); diff --git a/tools/transform_point_cloud.cpp b/tools/transform_point_cloud.cpp index e7dcfd6b7ae..9b7c51f5896 100644 --- a/tools/transform_point_cloud.cpp +++ b/tools/transform_point_cloud.cpp @@ -295,7 +295,7 @@ main (int argc, char** argv) const float& y = values[1]; const float& z = values[2]; const float& w = values[3]; - tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z)); + tform.topLeftCorner<3, 3> () = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z)); } else { @@ -312,7 +312,7 @@ main (int argc, char** argv) const float& ay = values[1]; const float& az = values[2]; const float& theta = values[3]; - tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az))); + tform.topLeftCorner<3, 3> () = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az))); } else { diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp index 0f746cabb83..6c0f42a5f54 100644 --- a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp +++ b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp @@ -1249,10 +1249,10 @@ pcl::visualization::PCLVisualizer::addCorrespondences ( Eigen::Affine3f source_transformation; source_transformation.linear () = source_points->sensor_orientation_.matrix (); - source_transformation.translation () = source_points->sensor_origin_.head (3); + source_transformation.translation () = source_points->sensor_origin_.template head<3> (); Eigen::Affine3f target_transformation; target_transformation.linear () = target_points->sensor_orientation_.matrix (); - target_transformation.translation () = target_points->sensor_origin_.head (3); + target_transformation.translation () = target_points->sensor_origin_.template head<3> (); int j = 0; // Draw lines between the best corresponding points diff --git a/visualization/src/interactor_style.cpp b/visualization/src/interactor_style.cpp index 3d6c23f956b..3d1a89c1437 100644 --- a/visualization/src/interactor_style.cpp +++ b/visualization/src/interactor_style.cpp @@ -225,7 +225,7 @@ pcl::visualization::PCLVisualizerInteractorStyle::setCameraParameters (const Eig Eigen::Vector3f pos_vec = extrinsics.block<3, 1> (0, 3); // Rotate the view vector - Eigen::Matrix3f rotation = extrinsics.block<3, 3> (0, 0); + Eigen::Matrix3f rotation = extrinsics.topLeftCorner<3, 3> (); Eigen::Vector3f y_axis (0.f, 1.f, 0.f); Eigen::Vector3f up_vec (rotation * y_axis); diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp index 5290114b631..fe1b15fabc1 100644 --- a/visualization/src/pcl_visualizer.cpp +++ b/visualization/src/pcl_visualizer.cpp @@ -4162,8 +4162,8 @@ pcl::visualization::PCLVisualizer::getTransformationMatrix ( Eigen::Matrix4f &transformation) { transformation.setIdentity (); - transformation.block<3, 3> (0, 0) = orientation.toRotationMatrix (); - transformation.block<3, 1> (0, 3) = origin.head (3); + transformation.topLeftCorner<3, 3> () = orientation.toRotationMatrix (); + transformation.block<3, 1> (0, 3) = origin.head<3> (); } //////////////////////////////////////////////////////////////////////////////////////////////