/* * Decomposes the orientation in euler angles so that this can be * obtained by applying the following rotations in order: * * rotation of a2 around x-axis * rotation of a1 around y-axis * rotation of a0 around z-axis * * assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2) * */ static base::Vector3d getEuler(const base::AngleAxisd &orientation){ const Eigen::Matrix3d m = orientation.toRotationMatrix(); double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm(); base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0); if (x > Eigen::NumTraits<double>::dummy_precision()){ res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0)); res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2)); }else{ res[0] = 0; res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1)); } return res; }
template <typename PointT> inline unsigned int pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, Eigen::Vector4d ¢roid) { // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero (); unsigned point_count; if (cloud.is_dense) { point_count = indices.size (); for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) { //const PointT& point = cloud[*iIt]; accu [0] += cloud[*iIt].x * cloud[*iIt].x; accu [1] += cloud[*iIt].x * cloud[*iIt].y; accu [2] += cloud[*iIt].x * cloud[*iIt].z; accu [3] += cloud[*iIt].y * cloud[*iIt].y; accu [4] += cloud[*iIt].y * cloud[*iIt].z; accu [5] += cloud[*iIt].z * cloud[*iIt].z; accu [6] += cloud[*iIt].x; accu [7] += cloud[*iIt].y; accu [8] += cloud[*iIt].z; } } else { point_count = 0; for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) { if (!isFinite (cloud[*iIt])) continue; ++point_count; accu [0] += cloud[*iIt].x * cloud[*iIt].x; accu [1] += cloud[*iIt].x * cloud[*iIt].y; accu [2] += cloud[*iIt].x * cloud[*iIt].z; accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4 accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5 accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8 accu [6] += cloud[*iIt].x; accu [7] += cloud[*iIt].y; accu [8] += cloud[*iIt].z; } } if (point_count != 0) { accu /= static_cast<double> (point_count); Eigen::Vector3f vec = accu.tail<3> (); centroid.head<3> () = vec;//= accu.tail<3> (); centroid[3] = 0; covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); } return (point_count); }
template <typename PointT> inline unsigned int pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix3d &covariance_matrix, Eigen::Vector4d ¢roid) { // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero (); unsigned int point_count; if (cloud.is_dense) { point_count = cloud.size (); // For each point in the cloud for (size_t i = 0; i < point_count; ++i) { accu [0] += cloud[i].x * cloud[i].x; accu [1] += cloud[i].x * cloud[i].y; accu [2] += cloud[i].x * cloud[i].z; accu [3] += cloud[i].y * cloud[i].y; accu [4] += cloud[i].y * cloud[i].z; accu [5] += cloud[i].z * cloud[i].z; accu [6] += cloud[i].x; accu [7] += cloud[i].y; accu [8] += cloud[i].z; } } else { point_count = 0; for (size_t i = 0; i < cloud.points.size (); ++i) { if (!isFinite (cloud[i])) continue; accu [0] += cloud[i].x * cloud[i].x; accu [1] += cloud[i].x * cloud[i].y; accu [2] += cloud[i].x * cloud[i].z; accu [3] += cloud[i].y * cloud[i].y; accu [4] += cloud[i].y * cloud[i].z; accu [5] += cloud[i].z * cloud[i].z; accu [6] += cloud[i].x; accu [7] += cloud[i].y; accu [8] += cloud[i].z; ++point_count; } } if (point_count != 0) { accu /= static_cast<double> (point_count); centroid.head<3> () = accu.tail<3> (); centroid[3] = 0; covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); } return (point_count); }