template <typename PointT> inline unsigned int pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix) { // 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, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero (); unsigned int 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; } } 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; accu [4] += cloud[*iIt].y * cloud[*iIt].z; accu [5] += cloud[*iIt].z * cloud[*iIt].z; } } if (point_count != 0) { accu /= static_cast<double> (point_count); covariance_matrix.coeffRef (0) = accu [0]; covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; covariance_matrix.coeffRef (4) = accu [3]; covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; covariance_matrix.coeffRef (8) = accu [5]; } return (point_count); }
template <typename PointT> inline unsigned int pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix3d &covariance_matrix) { // 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, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, 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; } } 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; ++point_count; } } if (point_count != 0) { accu /= static_cast<double> (point_count); covariance_matrix.coeffRef (0) = accu [0]; covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; covariance_matrix.coeffRef (4) = accu [3]; covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; covariance_matrix.coeffRef (8) = accu [5]; } return (point_count); }
void rpyToRotationMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& mat) { double sin_a = sin(rpy.coeff(2)); double cos_a = cos(rpy.coeff(2)); double sin_b = sin(rpy.coeff(1)); double cos_b = cos(rpy.coeff(1)); double sin_g = sin(rpy.coeff(0)); double cos_g = cos(rpy.coeff(0)); mat.coeffRef(0, 0) = cos_a * cos_b; mat.coeffRef(0, 1) = cos_a * sin_b * sin_g - sin_a * cos_g; mat.coeffRef(0, 2) = cos_a * sin_b * cos_g + sin_a * sin_g; mat.coeffRef(1, 0) = sin_a * cos_b; mat.coeffRef(1, 1) = sin_a * sin_b * sin_g + cos_a * cos_g; mat.coeffRef(1, 2) = sin_a * sin_b * cos_g - cos_a * sin_g; mat.coeffRef(2, 0) = -sin_b; mat.coeffRef(2, 1) = cos_b * sin_g; mat.coeffRef(2, 2) = cos_b * cos_g; }
void rpyToRotationMatrix(const std::vector<double>& rpy, Eigen::Matrix3d& mat) { double sin_a = sin(rpy[2]); double cos_a = cos(rpy[2]); double sin_b = sin(rpy[1]); double cos_b = cos(rpy[1]); double sin_g = sin(rpy[0]); double cos_g = cos(rpy[0]); mat.coeffRef(0, 0) = cos_a * cos_b; mat.coeffRef(0, 1) = cos_a * sin_b * sin_g - sin_a * cos_g; mat.coeffRef(0, 2) = cos_a * sin_b * cos_g + sin_a * sin_g; mat.coeffRef(1, 0) = sin_a * cos_b; mat.coeffRef(1, 1) = sin_a * sin_b * sin_g + cos_a * cos_g; mat.coeffRef(1, 2) = sin_a * sin_b * cos_g - cos_a * sin_g; mat.coeffRef(2, 0) = -sin_b; mat.coeffRef(2, 1) = cos_b * sin_g; mat.coeffRef(2, 2) = cos_b * cos_g; }
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 /= (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 /= (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); }