Go to the documentation of this file.00001 #include "zyonz_geometric_based_single_leaf_probing_alg.h"
00002 #include <pcl_ros/point_cloud.h>
00003 #include <pcl_ros/transforms.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl/filters/passthrough.h>
00006 #include <pcl/filters/extract_indices.h>
00007 #include <pcl/filters/project_inliers.h>
00008 #include <pcl/filters/voxel_grid.h>
00009 #include <pcl/surface/convex_hull.h>
00010 #include <pcl/surface/concave_hull.h>
00011 #include <pcl/sample_consensus/method_types.h>
00012 #include <pcl/segmentation/sac_segmentation.h>
00013 #include <pcl/features/boundary.h>
00014 #include <pcl/features/normal_3d.h>
00015 #include <pcl/kdtree/kdtree.h>
00016 #include <pcl/kdtree/kdtree_flann.h>
00017
00018 #include <tf/transform_broadcaster.h>
00019 #include <tf/tfMessage.h>
00020
00021 #include <visualization_msgs/Marker.h>
00022 #include <geometry_msgs/PoseArray.h>
00023
00024 ZyonzGeometricBasedSingleLeafProbingAlgorithm::ZyonzGeometricBasedSingleLeafProbingAlgorithm(void)
00025 {
00026 }
00027
00028 ZyonzGeometricBasedSingleLeafProbingAlgorithm::~ZyonzGeometricBasedSingleLeafProbingAlgorithm(void)
00029 {
00030 }
00031
00032 void ZyonzGeometricBasedSingleLeafProbingAlgorithm::config_update(Config& new_cfg, uint32_t level)
00033 {
00034 this->lock();
00035
00036
00037 this->config_=new_cfg;
00038
00039 this->unlock();
00040 }
00041
00042
00043
00044 bool ZyonzGeometricBasedSingleLeafProbingAlgorithm::get_leaf_probing_point(const sensor_msgs::PointCloud2::ConstPtr& msg)
00045 {
00046
00047
00048 pcl::PointCloud<pcl::PointXYZ>::Ptr temp (new pcl::PointCloud<pcl::PointXYZ>);
00049 pcl::fromROSMsg(*msg, *temp);
00050
00051 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00052
00053 pcl::PassThrough<pcl::PointXYZ> pass;
00054 pass.setInputCloud (temp);
00055 pass.filter (*cloud);
00056
00057 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00058 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00059
00060 pcl::SACSegmentation<pcl::PointXYZ> seg;
00061
00062 seg.setOptimizeCoefficients (true);
00063
00064 seg.setModelType (pcl::SACMODEL_PLANE);
00065 seg.setMethodType (pcl::SAC_RANSAC);
00066 seg.setDistanceThreshold (0.01);
00067 seg.setInputCloud (cloud);
00068 seg.segment (*inliers, *coefficients);
00069
00070
00071
00072
00073 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
00074 pcl::ProjectInliers<pcl::PointXYZ> proj;
00075 proj.setModelType (pcl::SACMODEL_PLANE);
00076 proj.setInputCloud (cloud);
00077 proj.setModelCoefficients (coefficients);
00078 proj.filter (*cloud_projected);
00079
00080
00081
00082
00083 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
00084 pcl::ConcaveHull<pcl::PointXYZ> chull;
00085
00086 chull.setInputCloud (cloud_projected);
00087 chull.setAlpha (0.1);
00088 chull.reconstruct (*cloud_hull);
00089
00090
00091 pcl::compute3DCentroid (*cloud, leaf_centroid_);
00092
00093
00094
00095
00096 Eigen::MatrixXf B(cloud->points.size(),3);
00097 int aa = 0;
00098 for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud->begin(); it != cloud->end(); ++it, ++aa){
00099 B.row(aa)[0] = it->x - leaf_centroid_(0);
00100 B.row(aa)[1] = it->y - leaf_centroid_(1);
00101 B.row(aa)[2] = it->z - leaf_centroid_(2);
00102
00103 }
00104
00105
00106
00107 Eigen::JacobiSVD<Eigen::MatrixXf> svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
00108 std::cout << "SingularValues are: \n" << svd.singularValues() << std::endl;
00109
00110 std::cout << "Right Singular Vectors are: \n" << svd.matrixV() << std::endl;
00111
00112 Eigen::Vector3f n_v(svd.matrixV().col(1));
00113 Eigen::Vector3f o_v(svd.matrixV().col(0));
00114 Eigen::Vector3f a_v(svd.matrixV().col(2));
00115
00116 if (a_v.z()<0){
00117 a_v = -a_v;
00118 n_v = -n_v;
00119 }
00120
00121 Eigen::Matrix3f rot_m;
00122 rot_m << n_v.x(), o_v.x(), a_v.x(),
00123 n_v.y(), o_v.y(), a_v.y(),
00124 n_v.z(), o_v.z(), a_v.z();
00125 leaf_pose_ = Eigen::Quaternionf (rot_m);
00126 ROS_INFO("Quaternion(x, y, z, w): (%f, %f, %f, %f)", leaf_pose_.x(), leaf_pose_.y(), leaf_pose_.z(), leaf_pose_.w());
00127
00128
00129 uint32_t shape = visualization_msgs::Marker::SPHERE;
00130
00131
00132 Marker_msg_.header.frame_id = msg->header.frame_id;
00133 Marker_msg_.header.stamp = ros::Time::now();
00134 Marker_msg_.ns = "basic_shapes";
00135 Marker_msg_.id = 0;
00136 Marker_msg_.type = shape;
00137 Marker_msg_.action = visualization_msgs::Marker::ADD;
00138 Marker_msg_.pose.position.x = leaf_centroid_.x();
00139 Marker_msg_.pose.position.y = leaf_centroid_.y();
00140 Marker_msg_.pose.position.z = leaf_centroid_.z();
00141 Marker_msg_.pose.orientation.x = leaf_pose_.x();
00142 Marker_msg_.pose.orientation.y = leaf_pose_.y();
00143 Marker_msg_.pose.orientation.z = leaf_pose_.z();
00144 Marker_msg_.pose.orientation.w = leaf_pose_.w();
00145 Marker_msg_.scale.x = svd.singularValues()[1]/(svd.singularValues()[0]*10);
00146 Marker_msg_.scale.y = svd.singularValues()[0]/(svd.singularValues()[0]*10);
00147 Marker_msg_.scale.z = svd.singularValues()[2]/(svd.singularValues()[0]*10);
00148 Marker_msg_.color.r = 0.0f;
00149 Marker_msg_.color.g = 1.0f;
00150 Marker_msg_.color.b = 0.0f;
00151 Marker_msg_.color.a = 1.0;
00152 Marker_msg_.lifetime = ros::Duration();
00153
00154
00155 geometry_msgs::PoseStamped temp_pose_st;
00156 temp_pose_st.header.stamp = ros::Time::now();
00157 temp_pose_st.header.frame_id = msg->header.frame_id;
00158 temp_pose_st.pose.position.x = leaf_centroid_.x() + n_v.x()*(n_v.norm()-0.97)/n_v.norm();
00159 temp_pose_st.pose.position.y = leaf_centroid_.y() + n_v.y()*(n_v.norm()-0.97)/n_v.norm();
00160 temp_pose_st.pose.position.z = leaf_centroid_.z() + n_v.z()*(n_v.norm()-0.97)/n_v.norm();
00161 temp_pose_st.pose.orientation.x = leaf_pose_.x();
00162 temp_pose_st.pose.orientation.y = leaf_pose_.y();
00163 temp_pose_st.pose.orientation.z = leaf_pose_.z();
00164 temp_pose_st.pose.orientation.w = leaf_pose_.w();
00165 try{
00166 tf_listener_.waitForTransform(std::string("wam_link0"), temp_pose_st.header.frame_id, ros::Time::now(), ros::Duration(1.0));
00167 tf_listener_.transformPose(std::string("wam_link0"), temp_pose_st, probing_step_.probing);
00168 }catch (tf::TransformException ex){
00169 ROS_ERROR("%s", ex.what());
00170 }
00171
00172
00173 temp_pose_st.header = msg->header;
00174 temp_pose_st.header.stamp = ros::Time::now();
00175 temp_pose_st.pose.position.x = temp_pose_st.pose.position.x + n_v.x()*(n_v.norm()-0.90)/n_v.norm();
00176 temp_pose_st.pose.position.y = temp_pose_st.pose.position.y + n_v.y()*(n_v.norm()-0.90)/n_v.norm();
00177 temp_pose_st.pose.position.z = temp_pose_st.pose.position.z + n_v.z()*(n_v.norm()-0.90)/n_v.norm();
00178 temp_pose_st.pose.orientation.x = leaf_pose_.x();
00179 temp_pose_st.pose.orientation.y = leaf_pose_.y();
00180 temp_pose_st.pose.orientation.z = leaf_pose_.z();
00181 temp_pose_st.pose.orientation.w = leaf_pose_.w();
00182 geometry_msgs::PoseStamped pre_prob_st;
00183 try{
00184 tf_listener_.waitForTransform(std::string("wam_link0"), temp_pose_st.header.frame_id, ros::Time::now(), ros::Duration(1.0));
00185 tf_listener_.transformPose(std::string("wam_link0"), temp_pose_st, pre_prob_st);
00186 }catch (tf::TransformException ex){
00187 ROS_ERROR("%s", ex.what());
00188 }
00189 probing_step_.pre_probing.header = msg->header;
00190 probing_step_.pre_probing.header.stamp = ros::Time::now();
00191 probing_step_.pre_probing.header.frame_id = std::string("wam_link0");
00192 probing_step_.pre_probing.poses.clear();
00193 probing_step_.pre_probing.poses.push_back(pre_prob_st.pose);
00194
00195
00196 probing_step_.post_probing = probing_step_.pre_probing;
00197
00198
00199
00200
00201
00202
00203
00204 return true;
00205 }
00206
00207 visualization_msgs::Marker ZyonzGeometricBasedSingleLeafProbingAlgorithm::getMarker(){
00208 return Marker_msg_;
00209 }
00210
00211 Eigen::Vector4f ZyonzGeometricBasedSingleLeafProbingAlgorithm::getLeafCentroid(){
00212 return leaf_centroid_;
00213 }
00214
00215 Eigen::Quaternionf ZyonzGeometricBasedSingleLeafProbingAlgorithm::getLeafPose(){
00216 return leaf_pose_;
00217 }
00218
00219 zyonz_msgs::ProbingSteps ZyonzGeometricBasedSingleLeafProbingAlgorithm::getProbingSteps(){
00220 return probing_step_;
00221 }
00222