void pushEigenMarker(pcl::PCA<T>& pca, int& marker_id, MarkerArray& marker_array, double scale, const std::string& frame_id) { Marker marker; marker.lifetime = ros::Duration(0.1); marker.header.frame_id = frame_id; marker.ns = "cluster_eigen"; marker.type = Marker::ARROW; marker.scale.y = 0.01; marker.scale.z = 0.01; marker.pose.position.x = pca.getMean().coeff(0); marker.pose.position.y = pca.getMean().coeff(1); marker.pose.position.z = pca.getMean().coeff(2); Eigen::Quaternionf qx, qy, qz; Eigen::Matrix3f ev = pca.getEigenVectors(); Eigen::Vector3f axis_x(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0)); Eigen::Vector3f axis_y(ev.coeff(0, 1), ev.coeff(1, 1), ev.coeff(2, 1)); Eigen::Vector3f axis_z(ev.coeff(0, 2), ev.coeff(1, 2), ev.coeff(2, 2)); qx.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_x); qy.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_y); qz.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_z); marker.id = marker_id++; marker.scale.x = pca.getEigenValues().coeff(0) * scale; marker.pose.orientation.x = qx.x(); marker.pose.orientation.y = qx.y(); marker.pose.orientation.z = qx.z(); marker.pose.orientation.w = qx.w(); marker.color.b = 0.0; marker.color.g = 0.0; marker.color.r = 1.0; marker.color.a = 1.0; marker_array.markers.push_back(marker); marker.id = marker_id++; marker.scale.x = pca.getEigenValues().coeff(1) * scale; marker.pose.orientation.x = qy.x(); marker.pose.orientation.y = qy.y(); marker.pose.orientation.z = qy.z(); marker.pose.orientation.w = qy.w(); marker.color.b = 0.0; marker.color.g = 1.0; marker.color.r = 0.0; marker_array.markers.push_back(marker); marker.id = marker_id++; marker.scale.x = pca.getEigenValues().coeff(2) * scale; marker.pose.orientation.x = qz.x(); marker.pose.orientation.y = qz.y(); marker.pose.orientation.z = qz.z(); marker.pose.orientation.w = qz.w(); marker.color.b = 1.0; marker.color.g = 0.0; marker.color.r = 0.0; marker_array.markers.push_back(marker); }
TEST(PCA, copy_constructor) { // Test copy constructor pcl::PCA<pcl::PointXYZ> pca_copy(pca); try { Eigen::Matrix3f eigen_vectors_copy = pca_copy.getEigenVectors (); Eigen::Matrix3f eigen_vectors = pca.getEigenVectors (); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) EXPECT_EQ (eigen_vectors (i,j), eigen_vectors_copy (i,j)); } catch (pcl::InitFailedException &e) { std::cerr << "something wrong" << std::endl; } }