void FeatureCalculate::pca(CloudPtr cloud,PlanSegment &plane)
{
Eigen::MatrixXf X(3,cloud->size());
double avr_x=0.0,avr_y=0.0,avr_z=0.0;
for (int i = 0; i <cloud->size(); i++) {
avr_x = avr_x * double(i) / (i + 1) + cloud->points[i].x / double(i + 1);
avr_y = avr_y * double(i) / (i + 1) + cloud->points[i].y / double(i + 1);
avr_z = avr_z * double(i) / (i + 1) + cloud->points[i].z / double(i + 1);
}
for (int i=0;i<cloud->size();i++)
{
X(0,i)=cloud->points[i].x-avr_x;
X(1,i)=cloud->points[i].y-avr_y;
X(2,i)=cloud->points[i].z-avr_z;
}
Eigen::MatrixXf XT(cloud->size(),3);
XT=X.transpose();
Eigen::Matrix3f XXT;
XXT=X*XT;
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (XXT, eigen_value, eigen_vector);
plane.normal.normal_x = eigen_vector [0];
plane.normal.normal_y = eigen_vector [1];
plane.normal.normal_z = eigen_vector [2];
plane.distance = - (plane.normal.normal_x * avr_x + plane.normal.normal_y * avr_y + plane.normal.normal_z * avr_z);
float eig_sum = XXT.coeff (0) + XXT.coeff (4) + XXT.coeff (8);
plane.min_value=eigen_value;
if (eig_sum != 0)
plane.normal.curvature = fabsf (eigen_value / eig_sum);
else
plane.normal.curvature = 0.0;
}
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
float &nx, float &ny, float &nz, float &curvature)
{
// Avoid getting hung on Eigen's optimizers
// for (int i = 0; i < 9; ++i)
// if (!pcl_isfinite (covariance_matrix.coeff (i)))
// {
// //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
// nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
// return;
// }
// Extract the smallest eigenvalue and its eigenvector
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
nx = eigen_vector [0];
ny = eigen_vector [1];
nz = eigen_vector [2];
// Compute the curvature surface change
float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
if (eig_sum != 0)
curvature = fabsf (eigen_value / eig_sum);
else
curvature = 0;
}
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);
}
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
typename pcl::PointCloud<T>::Ptr hand_cloud,
//typename pcl::PointCloud<T>::Ptr finger_cloud,
const std::string& frame_id,
tf::Vector3& hand_position,
tf::Vector3& arm_position,
tf::Vector3& arm_direction)
{
typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
pcl::fromROSMsg(cloud_msg, *cloud);
if((cloud->points.size() < g_config.min_cluster_size) ||
(cloud->points.size() > g_config.max_cluster_size))
return false;
pcl::PCA<T> pca;
pca.setInputCloud(cloud);
Eigen::Vector4f mean = pca.getMean();
if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;
Eigen::Vector3f eigen_value = pca.getEigenValues();
double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);
if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;
T search_point;
Eigen::Matrix3f ev = pca.getEigenVectors();
Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
main_axis.normalize();
arm_direction.setX(main_axis.coeff(0));
arm_direction.setY(main_axis.coeff(1));
arm_direction.setZ(main_axis.coeff(2));
arm_position.setX(mean.coeff(0));
arm_position.setY(mean.coeff(1));
arm_position.setZ(mean.coeff(2));
main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
search_point.x = main_axis.coeff(0);
search_point.y = main_axis.coeff(1);
search_point.z = main_axis.coeff(2);
//find hand
pcl::KdTreeFLANN<T> kdtree;
kdtree.setInputCloud(cloud);
//find the closet point from the serach_point
std::vector<int> point_indeices(1);
std::vector<float> point_distances(1);
if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
{
//update search point
search_point = cloud->points[point_indeices[0]];
//show seach point
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(search_point.x, search_point.y, search_point.z,
1.0, 0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
//hand
point_indeices.clear();
point_distances.clear();
kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
for (size_t i = 0; i < point_indeices.size (); ++i)
{
hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
hand_cloud->points.back().r = 255;
hand_cloud->points.back().g = 0;
hand_cloud->points.back().b = 0;
}
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*hand_cloud, centroid);
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
0.0, 1.0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
hand_position.setX(centroid.coeff(0));
hand_position.setY(centroid.coeff(1));
hand_position.setZ(centroid.coeff(2));
#if 0
//fingers
search_point.x = centroid.coeff(0);
search_point.y = centroid.coeff(1);
search_point.z = centroid.coeff(2);
std::vector<int> point_indeices_inner;
std::vector<float> point_distances_inner;
kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner);
std::vector<int> point_indeices_outter;
std::vector<float> point_distances_outter;
kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter);
//ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size());
std::vector<int>::iterator it;
for(size_t i = 0; i < point_indeices_inner.size(); i++)
{
it = std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]);
if(it != point_indeices_outter.end())
{
point_indeices_outter.erase(it);
}
}
//ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size());
//ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ());
for (size_t i = 0; i < point_indeices_outter.size (); ++i)
{
finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]);
finger_cloud->points.back().r = 255;
finger_cloud->points.back().g = 0;
finger_cloud->points.back().b = 0;
}
#endif
}
else
{
return false;
}
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id);
}
return true;
}
void
pcl::getCameraMatrixFromProjectionMatrix (
const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
Eigen::Matrix3f& camera_matrix)
{
Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> ();
Eigen::Matrix3f KR_KRT = KR * KR.transpose ();
Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8);
memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
camera_matrix.coeffRef (8) = 1.0;
if (camera_matrix.Flags & Eigen::RowMajorBit)
{
camera_matrix.coeffRef (2) = cam.coeff (2);
camera_matrix.coeffRef (5) = cam.coeff (5);
camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2)));
}
else
{
camera_matrix.coeffRef (6) = cam.coeff (2);
camera_matrix.coeffRef (7) = cam.coeff (5);
camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2)));
}
}
template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
Eigen::Matrix3f &covariance_matrix,
Eigen::Vector4f ¢roid)
{
// create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
size_t 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;
}
}
accu /= static_cast<float> (point_count);
//Eigen::Vector3f vec = accu.tail<3> ();
//centroid.head<3> () = vec;//= accu.tail<3> ();
//centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
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 (static_cast<unsigned int> (point_count));
}
template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix3f &covariance_matrix,
Eigen::Vector4f ¢roid)
{
// create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
unsigned 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; // 4
accu [4] += cloud[i].y * cloud[i].z; // 5
accu [5] += cloud[i].z * cloud[i].z; // 8
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;
}
}
accu /= (float) point_count;
if (point_count != 0)
{
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);
}
