Go to the documentation of this file.00001 #include "pointcloud_to_eigenvectors_alg.h"
00002
00003 PointcloudToEigenvectorsAlgorithm::PointcloudToEigenvectorsAlgorithm(void)
00004 {
00005 }
00006
00007 PointcloudToEigenvectorsAlgorithm::~PointcloudToEigenvectorsAlgorithm(void)
00008 {
00009 }
00010
00011 void PointcloudToEigenvectorsAlgorithm::config_update(Config& new_cfg, uint32_t level)
00012 {
00013 this->lock();
00014
00015
00016 this->config_=new_cfg;
00017
00018 this->unlock();
00019 }
00020
00021
00022
00023 bool PointcloudToEigenvectorsAlgorithm::get_eigenvectors(const sensor_msgs::PointCloud2::ConstPtr& msg)
00024 {
00025
00026
00027
00028 pcl::PointCloud<pcl::PointXYZ>::Ptr temp (new pcl::PointCloud<pcl::PointXYZ>);
00029 pcl::fromROSMsg(*msg, *temp);
00030
00031 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00032
00033 pcl::PassThrough<pcl::PointXYZ> pass;
00034 pass.setInputCloud (temp);
00035 pass.filter (*cloud);
00036
00037
00038 pcl::compute3DCentroid (*cloud, plane_centroid_);
00039
00040
00041
00042
00043 Eigen::MatrixXf B(cloud->points.size(),3);
00044 int aa = 0;
00045 for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud->begin(); it != cloud->end(); ++it, ++aa){
00046 B.row(aa)[0] = it->x - plane_centroid_(0);
00047 B.row(aa)[1] = it->y - plane_centroid_(1);
00048 B.row(aa)[2] = it->z - plane_centroid_(2);
00049
00050 }
00051
00052
00053
00054 Eigen::JacobiSVD<Eigen::MatrixXf> svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
00055 std::cout << "SingularValues are: \n" << svd.singularValues() << std::endl;
00056
00057 std::cout << "Right Singular Vectors are: \n" << svd.matrixV() << std::endl;
00058
00059 Eigen::Vector3f n_v(svd.matrixV().col(1));
00060 Eigen::Vector3f o_v(svd.matrixV().col(0));
00061 Eigen::Vector3f a_v(svd.matrixV().col(2));
00062
00063 if (a_v.z()>0){
00064 a_v = -a_v;
00065 n_v = -n_v;
00066 }
00067
00068 Eigen::Matrix3f rot_m;
00069 rot_m << n_v.x(), o_v.x(), a_v.x(),
00070 n_v.y(), o_v.y(), a_v.y(),
00071 n_v.z(), o_v.z(), a_v.z();
00072 plane_pose_ = Eigen::Quaternionf (rot_m);
00073 ROS_INFO("Quaternion(x, y, z, w): (%f, %f, %f, %f)", plane_pose_.x(), plane_pose_.y(), plane_pose_.z(), plane_pose_.w());
00074
00075
00076 uint32_t shape = visualization_msgs::Marker::SPHERE;
00077
00078
00079 Marker_msg_.header.frame_id = msg->header.frame_id;
00080 Marker_msg_.header.stamp = ros::Time::now();
00081 Marker_msg_.ns = "basic_shapes";
00082 Marker_msg_.id = 0;
00083 Marker_msg_.type = shape;
00084 Marker_msg_.action = visualization_msgs::Marker::ADD;
00085 Marker_msg_.pose.position.x = plane_centroid_.x();
00086 Marker_msg_.pose.position.y = plane_centroid_.y();
00087 Marker_msg_.pose.position.z = plane_centroid_.z();
00088 Marker_msg_.pose.orientation.x = plane_pose_.x();
00089 Marker_msg_.pose.orientation.y = plane_pose_.y();
00090 Marker_msg_.pose.orientation.z = plane_pose_.z();
00091 Marker_msg_.pose.orientation.w = plane_pose_.w();
00092 Marker_msg_.scale.x = svd.singularValues()[1]/(svd.singularValues()[0]*10);
00093 Marker_msg_.scale.y = svd.singularValues()[0]/(svd.singularValues()[0]*10);
00094 Marker_msg_.scale.z = svd.singularValues()[2]/(svd.singularValues()[0]*10);
00095 Marker_msg_.color.r = 0.0f;
00096 Marker_msg_.color.g = 1.0f;
00097 Marker_msg_.color.b = 0.0f;
00098 Marker_msg_.color.a = 1.0;
00099 Marker_msg_.lifetime = ros::Duration();
00100
00101
00102 return true;
00103 }
00104
00105 visualization_msgs::Marker PointcloudToEigenvectorsAlgorithm::getMarker(){
00106 return Marker_msg_;
00107 }
00108
00109 Eigen::Vector4f PointcloudToEigenvectorsAlgorithm::getPlaneCentroid(){
00110 return plane_centroid_;
00111 }
00112
00113 Eigen::Quaternionf PointcloudToEigenvectorsAlgorithm::getPlanePose(){
00114 return plane_pose_;
00115 }
00116