hector_stair_detection.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <hector_stair_detection/hector_stair_detection.h>
00003 
00004 namespace hector_stair_detection{
00005 HectorStairDetection::HectorStairDetection(){
00006     ROS_INFO ("HectorStairDetection started");
00007     ros::NodeHandle nh("");
00008 
00009     //load params
00010     nh.param("passThroughZMin", passThroughZMin_, 0.1);
00011     nh.param("passThroughZMax", passThroughZMax_, 2.5);
00012     nh.param("voxelGridX", voxelGridX_, 0.04);
00013     nh.param("voxelGridY", voxelGridY_, 0.04);
00014     nh.param("voxelGridZ", voxelGridZ_, 0.04);
00015     nh.param("minRequiredPointsOnLine", minRequiredPointsOnLine_, 3);
00016     nh.param("distanceToLineTresh", distanceToLineTresh_, 0.1);
00017     nh.param("refineSurfaceRequired", refineSurfaceRequired_, false);
00018     nh.param("worldFrame", worldFrame_, std::string("/world")); //has to be true in hector-setup
00019     nh.param("culsterHeightTresh", clusterHeightTresh_, 0.1);
00020     nh.param("clusterTolerance", clusterTolerance_, 0.075);
00021     nh.param("clusterMinSize", clusterMinSize_, 50);
00022     nh.param("clusterMaxSize", clusterMaxSize_, 200);
00023     nh.param("setup", setup_, std::string("argo"));
00024     nh.param("maxClusterXYDimension", maxClusterXYDimension_, 1.0);
00025     nh.param("minHightDistBetweenAllStairsPoints", minHightDistBetweenAllStairsPoints_, 0.2);
00026     nh.param("maxDistBetweenStairsPoints", maxDistBetweenStairsPoints_, 3.0);
00027     nh.param("planeSegDistTresh", planeSegDistTresh_, 0.01);
00028     nh.param("planeSegAngleEps", planeSegAngleEps_, 0.1);
00029     nh.param("hesseTresh", hesseTresh_, 0.1);
00030 
00031     possible_stairs_cloud_pub_= nh.advertise<pcl::PointCloud<pcl::PointXYZI> >("/hector_stair_detection/possible_stairs_cloud", 100, true);
00032     points_on_line_cloud_debug_= nh.advertise<pcl::PointCloud<pcl::PointXYZI> >("/hector_stair_detection/point_on_line_debug", 100, true);
00033     surfaceCloud_pub_debug_= nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/surfaceCloud_debug", 100, true);
00034     final_stairs_cloud_pub_=  nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/final_stairs_cloud", 100, true);
00035     border_of_stairs_pub_= nh.advertise<visualization_msgs::MarkerArray>("/hector_stair_detection/boarder_of_stairs", 100, true);
00036     stairs_position_and_orientaion_pub_= nh.advertise<geometry_msgs::PoseStamped>("/hector_stair_detection/stairs_orientation", 100, true);
00037     stairs_position_and_orientaion_with_direction_pub_= nh.advertise<hector_stair_detection_msgs::PositionAndOrientaion>("/hector_stair_detection/stairs_orientaion_as_vector", 100, true);
00038     border_and_orientation_stairs_combined_pub_= nh.advertise<hector_stair_detection_msgs::BorderAndOrientationOfStairs>("/hector_stair_detection/border_and_orientation_of_stairs", 100, true);
00039     cloud_after_plane_detection_debug_pub_= nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/cloud_after_plane_detection_debug_", 100, true);
00040     line_marker_pub_= nh.advertise<visualization_msgs::Marker>("/hector_stair_detection/stairs_normal", 100, true);
00041 
00042     temp_orginal_pub_=nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/temp_original", 100, true);
00043     temp_after_pass_trough_pub_=nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/temp_after_pass_trough", 100, true);
00044     temp_after_voxel_grid_pub_=nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/hector_stair_detection/temp_after_voxel_grid", 100, true);
00045     temp_after_mls_pub_=nh.advertise<pcl::PointCloud<pcl::PointNormal> >("/hector_stair_detection/temp_after_mls", 100, true);
00046 
00047 
00048 
00049     if(setup_.compare(std::string("argo"))==0){
00050         //TODO:: get pointcloud from Service
00051         //        pointcloud_srv_client_ = nh.serviceClient<vigir_perception_msgs::PointCloudRegionRequest>("/flor/worldmodel/pointcloud_roi");
00052         //        vigir_perception_msgs::PointCloudRegionRequest srv;
00053         //        vigir_perception_msgs::EnvironmentRegionRequest erreq;
00054         //        erreq.header=worldFrame_;
00055         //        erreq.bounding_box_max.x=;
00056         //        erreq.bounding_box_max.y=;
00057         //        erreq.bounding_box_max.z=passThroughZMax_;
00058         //        erreq.bounding_box_min.x=;
00059         //        erreq.bounding_box_min.y=;
00060         //        erreq.bounding_box_min.z=passThroughZMin_;
00061         //        erreq.resolution=voxelGridX_;
00062         //        erreq.request_augment=0;
00063         //        srv.request.region_req=erreq;
00064         //        srv.request.aggregation_size=500;
00065         //        if(!pointcloud_srv_client_.call(srv)){
00066         //            ROS_ERROR("/flor/worldmodel/pointcloud_roi is not working");
00067         //        }else{
00068         //            sensor_msgs::PointCloud2 pointCloud_world;
00069         //            pointCloud_world=srv.response.cloud;
00070         pcl_sub = nh.subscribe("/worldmodel_main/pointcloud_vis", 10, &HectorStairDetection::PclCallback, this);
00071     }else{
00072         //        pcl_sub = nh.subscribe("/openni/depth/points", 1, &HectorStairDetection::PclCallback, this);
00073         //        pcl_sub = nh.subscribe("/hector_octomap_server/octomap_point_cloud_centers", 1, &HectorStairDetection::PclCallback, this);
00074         pcl_sub = nh.subscribe("/hector_aggregate_cloud/aggregated_cloud", 1, &HectorStairDetection::PclCallback, this);
00075     }
00076 
00077 }
00078 
00079 HectorStairDetection::~HectorStairDetection()
00080 {}
00081 
00082 void HectorStairDetection::publishResults(pcl::PointCloud<pcl::PointNormal>::Ptr &input_surface_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &planeCloud,
00083                                           pcl::IndicesClustersPtr cluster_indices, std::vector<int> final_cluster_idx, Eigen::Vector3f base, Eigen::Vector3f point){
00084 
00085     geometry_msgs::PoseStamped position_and_orientaion;
00086     Eigen::Vector3f directionStairs;
00087     getStairsPositionAndOrientation(base, point, input_surface_cloud->header.frame_id, directionStairs, position_and_orientaion);
00088     hector_stair_detection_msgs::PositionAndOrientaion pos_and_orientaion_message;
00089     pos_and_orientaion_message.orientation_of_stairs=position_and_orientaion;
00090     pos_and_orientaion_message.directionX=directionStairs(0);
00091     pos_and_orientaion_message.directionY=directionStairs(1);
00092     pos_and_orientaion_message.directionZ=directionStairs(2);
00093 
00094     //compute and pubish max and min boarder of the stairs
00095     pcl::PointCloud<pcl::PointXYZ>::Ptr final_stairsCloud(new pcl::PointCloud<pcl::PointXYZ>);
00096     visualization_msgs::MarkerArray stairs_boarder_marker;
00097     getFinalStairsCloud_and_position(input_surface_cloud->header.frame_id, directionStairs, input_surface_cloud, planeCloud, cluster_indices, final_cluster_idx, final_stairsCloud, stairs_boarder_marker, base);
00098 
00099     if(final_stairs_cloud_pub_.getNumSubscribers()>0){
00100         final_stairs_cloud_pub_.publish(final_stairsCloud);
00101     }
00102     if(border_of_stairs_pub_.getNumSubscribers()>0){
00103         border_of_stairs_pub_.publish(stairs_boarder_marker);
00104     }
00105 
00106     hector_stair_detection_msgs::BorderAndOrientationOfStairs border_and_orientation_msg;
00107     border_and_orientation_msg.header.frame_id=input_surface_cloud->header.frame_id;
00108     border_and_orientation_msg.border_of_stairs= stairs_boarder_marker;
00109     border_and_orientation_msg.orientation_of_stairs=position_and_orientaion;
00110     border_and_orientation_msg.number_of_points= final_cluster_idx.size();
00111     border_and_orientation_msg.directionX=directionStairs(0);
00112     border_and_orientation_msg.directionY=directionStairs(1);
00113     border_and_orientation_msg.directionZ=directionStairs(2);
00114 
00115     if(border_and_orientation_stairs_combined_pub_.getNumSubscribers()>0){
00116         border_and_orientation_stairs_combined_pub_.publish(border_and_orientation_msg);
00117     }
00118 
00119     if(stairs_position_and_orientaion_with_direction_pub_.getNumSubscribers()>0){
00120         stairs_position_and_orientaion_with_direction_pub_.publish(pos_and_orientaion_message);
00121     }
00122     if(stairs_position_and_orientaion_pub_.getNumSubscribers()>0){
00123         stairs_position_and_orientaion_pub_.publish(position_and_orientaion);
00124     }
00125 }
00126 
00127 void HectorStairDetection::getStairsPositionAndOrientation(Eigen::Vector3f &base, Eigen::Vector3f point, std::string frameID, Eigen::Vector3f &direction, geometry_msgs::PoseStamped &position_and_orientaion){
00128     Eigen::Vector3f stairs_position= 0.5*(base + point);
00129 
00130     position_and_orientaion.header.frame_id=frameID;
00131     position_and_orientaion.pose.position.x=stairs_position(0);
00132     position_and_orientaion.pose.position.y=stairs_position(1);
00133     position_and_orientaion.pose.position.z=stairs_position(2);
00134 
00135     if(point(2) >=base(2)){
00136         direction=point-base;
00137     }else{
00138         direction=base-point;
00139         base=point;
00140     }
00141     float stairs_yaw= atan2(direction(1), direction(0));
00142     float staris_pitch= atan2(direction(1)*sin(stairs_yaw)+direction(0)*cos(stairs_yaw), direction(2))+M_PI_2;
00143     tf::Quaternion temp;
00144     temp.setEulerZYX(stairs_yaw,staris_pitch,0.0);
00145     position_and_orientaion.pose.orientation.x=temp.getX();
00146     position_and_orientaion.pose.orientation.y=temp.getY();
00147     position_and_orientaion.pose.orientation.z=temp.getZ();
00148     position_and_orientaion.pose.orientation.w=temp.getW();
00149 }
00150 
00151 void HectorStairDetection::getFinalStairsCloud_and_position(std::string frameID, Eigen::Vector3f directionS, pcl::PointCloud<pcl::PointNormal>::Ptr &input_surface_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &planeCloud, pcl::IndicesClustersPtr cluster_indices, std::vector<int> final_cluster_idx,
00152                                                             pcl::PointCloud<pcl::PointXYZ>::Ptr &final_stairsCloud, visualization_msgs::MarkerArray &stairs_boarder_marker, Eigen::Vector3f base){
00153     int cluster_counter=0;
00154     float minZ= FLT_MAX;
00155     float maxZ= -FLT_MAX;
00156     int minZIndex=-1;
00157     int maxZIndex=-1;
00158     final_stairsCloud->header.frame_id=frameID;
00159 //    make use of the plane cloud
00160 //    if(planeCloud->size() !=0){
00161 //        Eigen::Vector3f orthogonalDir=directionS.cross(Eigen::Vector3f(0,0,1));
00162 //        Eigen::Vector3f point;
00163 //        pcl::PointCloud<pcl::PointXYZ>::Ptr testCloud(new pcl::PointCloud<pcl::PointXYZ>());
00164 //        testCloud->header.frame_id=frameID;
00165 //        for(int i=0; i<planeCloud->size(); i++){
00166 //            point(0)=planeCloud->points.at(i).x;
00167 //            point(1)=planeCloud->points.at(i).y;
00168 //            point(2)=planeCloud->points.at(i).z;
00169 //            //TODO:: check angle
00171 //            float angle=acos((orthogonalDir.dot(point-base))/(orthogonalDir.norm()*(point-base).norm()));
00172 
00173 //            ROS_INFO("ange: %f", angle);
00174 //            if(angle < M_PI_2){
00175 //                testCloud->push_back(planeCloud->points.at(i));
00176 //            }
00177 
00178 //            //compute distance
00179 //            float tempA= std::sqrt(directionS.cross(point-base).dot(directionS.cross(point-base)));
00180 //            float lengthDirection= std::sqrt(directionS.dot(directionS));
00181 //            float distance= tempA/lengthDirection;
00182 
00183 //            //save max depening on sign
00184 
00185 
00186 //        }
00187 //        cloud_after_plane_detection_debug_pub_.publish(testCloud);
00188 //    }
00189 
00190     for (int i = 0; i < cluster_indices->size (); ++i){
00191         std::vector<int>::iterator idx_it;
00192 
00193         idx_it = find (final_cluster_idx.begin(), final_cluster_idx.end(), cluster_counter);
00194         if (idx_it != final_cluster_idx.end()){
00195             for (int j = 0; j < (*cluster_indices)[i].indices.size (); ++j){
00196                 pcl::PointXYZ tempP;
00197                 tempP.x=input_surface_cloud->points[(*cluster_indices)[i].indices[j]].x;
00198                 tempP.y=input_surface_cloud->points[(*cluster_indices)[i].indices[j]].y;
00199                 tempP.z=input_surface_cloud->points[(*cluster_indices)[i].indices[j]].z;
00200                 final_stairsCloud->points.push_back(tempP);
00201 
00202                 if(tempP.z < minZ){
00203                     minZ=tempP.z;
00204                     minZIndex=i;
00205                 }
00206 
00207                 if(tempP.z > maxZ){
00208                     maxZ=tempP.z;
00209                     maxZIndex=i;
00210                 }
00211             }
00212         }
00213         cluster_counter=cluster_counter+1;
00214     }
00215 
00216     Eigen::Vector3f minStepAVG;
00217     minStepAVG(0)=0;
00218     minStepAVG(1)=0;
00219     minStepAVG(2)=0;
00220     Eigen::Vector3f maxStepAVG;
00221     maxStepAVG(0)=0;
00222     maxStepAVG(1)=0;
00223     maxStepAVG(2)=0;
00224 
00225 
00226     for (int j = 0; j < (*cluster_indices)[minZIndex].indices.size (); ++j){
00227         minStepAVG(0)=minStepAVG(0)+input_surface_cloud->points[(*cluster_indices)[minZIndex].indices[j]].x;
00228         minStepAVG(1)=minStepAVG(1)+input_surface_cloud->points[(*cluster_indices)[minZIndex].indices[j]].y;
00229         minStepAVG(2)=minStepAVG(2)+input_surface_cloud->points[(*cluster_indices)[minZIndex].indices[j]].z;
00230     }
00231 
00232     minStepAVG=minStepAVG/(*cluster_indices)[minZIndex].indices.size ();
00233 
00234     for (int j = 0; j < (*cluster_indices)[maxZIndex].indices.size (); ++j){
00235         maxStepAVG(0)=maxStepAVG(0)+input_surface_cloud->points[(*cluster_indices)[maxZIndex].indices[j]].x;
00236         maxStepAVG(1)=maxStepAVG(1)+input_surface_cloud->points[(*cluster_indices)[maxZIndex].indices[j]].y;
00237         maxStepAVG(2)=maxStepAVG(2)+input_surface_cloud->points[(*cluster_indices)[maxZIndex].indices[j]].z;
00238     }
00239 
00240     maxStepAVG=maxStepAVG/(*cluster_indices)[maxZIndex].indices.size ();
00241 
00242 
00243     for(int i=0; i<4; i++){
00244         visualization_msgs::Marker marker;
00245         marker.header.frame_id = frameID;
00246         marker.type = visualization_msgs::Marker::SPHERE;
00247         marker.action = visualization_msgs::Marker::ADD;
00248         marker.ns = "hector_stair_detection";
00249         marker.id = i;
00250         if((directionS(0)> 0 && directionS(1) >0) || (directionS(0)< 0 && directionS(1) <0)){
00251             if(i==0){
00252                 marker.pose.position.x = minStepAVG(0) - 0.5*sin(atan2(directionS(1), directionS(0)));
00253                 marker.pose.position.y = minStepAVG(1) + 0.5*cos(atan2(directionS(1), directionS(0)));
00254                 marker.pose.position.z=minStepAVG(2);
00255             }
00256 
00257             if(i==1){
00258                 marker.pose.position.x = minStepAVG(0) + 0.5*sin(atan2(directionS(1), directionS(0)));
00259                 marker.pose.position.y = minStepAVG(1) - 0.5*cos(atan2(directionS(1), directionS(0)));
00260                 marker.pose.position.z=minStepAVG(2);
00261             }
00262 
00263             if(i==2){
00264                 marker.pose.position.x = maxStepAVG(0) - 0.5*sin(atan2(directionS(1), directionS(0)));
00265                 marker.pose.position.y = maxStepAVG(1) + 0.5*cos(atan2(directionS(1), directionS(0)));
00266                 marker.pose.position.z=maxStepAVG(2);
00267             }
00268 
00269             if(i==3){
00270                 marker.pose.position.x = maxStepAVG(0) + 0.5*sin(atan2(directionS(1), directionS(0)));
00271                 marker.pose.position.y = maxStepAVG(1) - 0.5*cos(atan2(directionS(1), directionS(0)));
00272                 marker.pose.position.z=maxStepAVG(2);
00273             }
00274         }else{
00275             if(i==0){
00276                 marker.pose.position.x = minStepAVG(0) + 0.5*sin(atan2(directionS(1), directionS(0)));
00277                 marker.pose.position.y = minStepAVG(1) + 0.5*cos(atan2(directionS(1), directionS(0)));
00278                 marker.pose.position.z=minStepAVG(2);
00279             }
00280 
00281             if(i==1){
00282                 marker.pose.position.x = minStepAVG(0) - 0.5*sin(atan2(directionS(1), directionS(0)));
00283                 marker.pose.position.y = minStepAVG(1) - 0.5*cos(atan2(directionS(1), directionS(0)));
00284                 marker.pose.position.z=minStepAVG(2);
00285             }
00286 
00287             if(i==2){
00288                 marker.pose.position.x = maxStepAVG(0) + 0.5*sin(atan2(directionS(1), directionS(0)));
00289                 marker.pose.position.y = maxStepAVG(1) + 0.5*cos(atan2(directionS(1), directionS(0)));
00290                 marker.pose.position.z=maxStepAVG(2);
00291             }
00292 
00293             if(i==3){
00294                 marker.pose.position.x = maxStepAVG(0) - 0.5*sin(atan2(directionS(1), directionS(0)));
00295                 marker.pose.position.y = maxStepAVG(1) - 0.5*cos(atan2(directionS(1), directionS(0)));
00296                 marker.pose.position.z=maxStepAVG(2);
00297             }
00298         }
00299 
00300         marker.pose.orientation.x = 0.0;
00301         marker.pose.orientation.y = 0.0;
00302         marker.pose.orientation.z = 0.0;
00303         marker.pose.orientation.w = 1.0;
00304         marker.scale.x = 0.2;
00305         marker.scale.y = 0.2;
00306         marker.scale.z = 0.2;
00307         marker.color.a = 1.0;
00308         marker.color.r = 0.0;
00309         marker.color.g = 1.0;
00310         marker.color.b = 0.0;
00311 
00312         stairs_boarder_marker.markers.push_back(marker);
00313 
00314     }
00315         projectStairsToFloor(directionS, stairs_boarder_marker);
00316 }
00317 
00318 void HectorStairDetection::projectStairsToFloor(Eigen::Vector3f direction, visualization_msgs::MarkerArray &stairs_boarder_marker){
00319     float first_step_z=0.05;
00320     float minZ= FLT_MAX;
00321     int minPos1;
00322     int minPos2;
00323     for(int i=0; i<stairs_boarder_marker.markers.size(); i++){
00324         if(stairs_boarder_marker.markers.at(i).pose.position.z <minZ){
00325             minZ=stairs_boarder_marker.markers.at(i).pose.position.z;
00326             minPos1=i;
00327         }
00328     }
00329 
00330     minZ= FLT_MAX;
00331     for(int i=0; i<stairs_boarder_marker.markers.size(); i++){
00332         if(i != minPos1 && stairs_boarder_marker.markers.at(i).pose.position.z <minZ){
00333             minZ=stairs_boarder_marker.markers.at(i).pose.position.z;
00334             minPos2=i;
00335         }
00336     }
00337 
00338     stairs_boarder_marker.markers.at(minPos1).pose.position.x=stairs_boarder_marker.markers.at(minPos1).pose.position.x+(first_step_z-stairs_boarder_marker.markers.at(minPos1).pose.position.z)/direction(2)*direction(0);
00339     stairs_boarder_marker.markers.at(minPos1).pose.position.y=stairs_boarder_marker.markers.at(minPos1).pose.position.y+(first_step_z-stairs_boarder_marker.markers.at(minPos1).pose.position.z)/direction(2)*direction(1);
00340     stairs_boarder_marker.markers.at(minPos1).pose.position.z=first_step_z;
00341     stairs_boarder_marker.markers.at(minPos2).pose.position.x=stairs_boarder_marker.markers.at(minPos2).pose.position.x+(first_step_z-stairs_boarder_marker.markers.at(minPos2).pose.position.z)/direction(2)*direction(0);
00342     stairs_boarder_marker.markers.at(minPos2).pose.position.y=stairs_boarder_marker.markers.at(minPos2).pose.position.y+(first_step_z-stairs_boarder_marker.markers.at(minPos2).pose.position.z)/direction(2)*direction(1);
00343     stairs_boarder_marker.markers.at(minPos2).pose.position.z=first_step_z;
00344 }
00345 
00346 int HectorStairDetection::getZComponent(Eigen::Vector2f directionStairs, Eigen::Vector2f minXminY, Eigen::Vector2f maxXminY, Eigen::Vector2f minXmaxY){
00347     Eigen::Vector2f direction1;
00348     Eigen::Vector2f direction2;
00349     Eigen::Vector2f direction3;
00350     Eigen::Vector2f direction4;
00351 
00352     direction1=maxXminY-minXminY;
00353     direction2=minXmaxY-minXminY;
00354     direction3=minXminY-maxXminY;
00355     direction4=minXminY-minXmaxY;
00356 
00357     float angle1=acos((directionStairs.dot(direction1))/(directionStairs.norm()*direction1.norm()));
00358     float angle2=acos((directionStairs.dot(direction2))/(directionStairs.norm()*direction2.norm()));
00359     float angle3=acos((directionStairs.dot(direction3))/(directionStairs.norm()*direction3.norm()));
00360     float angle4=acos((directionStairs.dot(direction4))/(directionStairs.norm()*direction4.norm()));
00361 
00362     if(angle1>=angle2 && angle1>=angle3 && angle1>=angle4){
00363         return 1;
00364     }
00365 
00366     if(angle2>=angle1 && angle2>=angle3 && angle2>=angle4){
00367         return 2;
00368     }
00369 
00370     if(angle3>=angle2 && angle3>=angle1 && angle3>=angle4){
00371         return 3;
00372     }
00373 
00374     if(angle4>=angle2 && angle4>=angle3 && angle4>=angle1){
00375         return 4;
00376     }
00377 }
00378 
00379 void HectorStairDetection::getPreprocessedCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud, pcl::PointCloud<pcl::PointNormal>::Ptr &output_cloud){
00380     ROS_INFO("Hector Stair Detection get Surface");
00381     pcl::PointCloud<pcl::PointXYZ>::Ptr processCloud_v1(new pcl::PointCloud<pcl::PointXYZ>());
00382     pcl::PointCloud<pcl::PointXYZ>::Ptr processCloud_v2(new pcl::PointCloud<pcl::PointXYZ>());
00383 
00384     temp_orginal_pub_.publish(input_cloud);
00385 
00386     pcl::PassThrough<pcl::PointXYZ> pass;
00387     pass.setInputCloud(input_cloud);
00388     pass.setFilterFieldName("z");
00389     pass.setFilterLimits(passThroughZMin_, passThroughZMax_);
00390     pass.filter(*processCloud_v1);
00391 
00392 //            pass.setInputCloud(processCloud_v1);
00393 //            pass.setFilterFieldName("y");
00394 //            pass.setFilterLimits(-1.0, 1.0);
00395 //            pass.filter(*processCloud_v1);
00396 
00397 //            pass.setInputCloud(processCloud_v1);
00398 //            pass.setFilterFieldName("x");
00399 //            pass.setFilterLimits(0.0, 3.5);
00400 //            pass.filter(*processCloud_v1);
00401 
00402     temp_after_pass_trough_pub_.publish(processCloud_v1);
00403 
00404     pcl::VoxelGrid<pcl::PointXYZ> vox;
00405     vox.setInputCloud(processCloud_v1);
00406     vox.setLeafSize(voxelGridX_, voxelGridY_, voxelGridZ_);
00407     vox.setDownsampleAllData(false);
00408     vox.filter(*processCloud_v2);
00409 
00410     temp_after_voxel_grid_pub_.publish(processCloud_v2);
00411 
00412     pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
00413     mls.setSearchRadius(0.05);
00414     mls.setPolynomialOrder(1);
00415     mls.setComputeNormals(true);
00416     mls.setInputCloud(processCloud_v2);
00417 
00418     boost::shared_ptr<pcl::PointCloud<pcl::PointNormal> > point_cloud_mls_normal;
00419     point_cloud_mls_normal.reset(new pcl::PointCloud<pcl::PointNormal>);
00420     mls.process(*point_cloud_mls_normal);
00421 
00422     temp_after_mls_pub_.publish(point_cloud_mls_normal);
00423 
00424     //    pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
00425     //    ne.setInputCloud (processCloud_v2);
00426     //      pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
00427     //      ne.setSearchMethod (tree);
00428     //      // Output datasets
00429     //      pcl::PointCloud<pcl::PointNormal>::Ptr point_cloud_mls_normal (new pcl::PointCloud<pcl::PointNormal>);
00430     //      // Use all neighbors in a sphere of radius 3cm
00431     //      ne.setRadiusSearch (0.03);
00432     //      // Compute the features
00433     //      ne.compute (*point_cloud_mls_normal);
00434 
00435     //    temp_after_mls_pub_.publish(point_cloud_mls_normal);
00436 
00437     output_cloud->clear();
00438     output_cloud->header.frame_id=input_cloud->header.frame_id;
00439 
00440     output_cloud.reset(new pcl::PointCloud<pcl::PointNormal>(*point_cloud_mls_normal));
00441 
00442 
00443     std::vector<int> indices;
00444     pcl::removeNaNFromPointCloud(*output_cloud,*output_cloud, indices);
00445     pcl::removeNaNNormalsFromPointCloud(*output_cloud,*output_cloud, indices);
00446 
00447     if(surfaceCloud_pub_debug_.getNumSubscribers()>0){
00448         surfaceCloud_pub_debug_.publish(output_cloud);
00449     }
00450 }
00451 
00452 bool customRegionGrowing (const pcl::PointNormal& point_a, const pcl::PointNormal& point_b, float squared_distance){
00453     Eigen::Map<const Eigen::Vector3f> point_b_normal = point_b.normal;
00454 
00455     Eigen::Vector3f d1(1,0,0);
00456     Eigen::Vector3f d2(0,1,0);
00457 
00458     if(fabs(d1.dot(point_b_normal)) < 0.2 && fabs(d2.dot(point_b_normal)) < 0.2){
00459         return true;
00460     }
00461     return false;
00462 }
00463 
00464 
00465 void HectorStairDetection::PclCallback(const sensor_msgs::PointCloud2::ConstPtr& pc_msg){
00466     ROS_INFO("stairs position callback enterd");
00467     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00468     pcl::PCLPointCloud2 pcl_pc2;
00469     pcl_conversions::toPCL(*pc_msg,pcl_pc2);
00470     pcl::fromPCLPointCloud2(pcl_pc2,*input_cloud);
00471     //transform cloud to /world
00472     tf::StampedTransform transform_cloud_to_map;
00473     try{
00474         ros::Time time = pc_msg->header.stamp;
00475         listener_.waitForTransform(worldFrame_, pc_msg->header.frame_id,
00476                                    time, ros::Duration(3.0));
00477         listener_.lookupTransform(worldFrame_, pc_msg->header.frame_id,
00478                                   time, transform_cloud_to_map);
00479     }
00480     catch (tf::TransformException ex){
00481         ROS_ERROR("Lookup Transform failed: %s",ex.what());
00482         return;
00483     }
00484 
00485     tf::transformTFToEigen(transform_cloud_to_map, to_map_);
00486 
00487     // Transform to /world
00488     boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
00489     pcl::transformPointCloud(*input_cloud, *cloud_tmp, to_map_);
00490     input_cloud = cloud_tmp;
00491     input_cloud->header.frame_id= transform_cloud_to_map.frame_id_;
00492 
00493     float clusterHeightTresh=clusterHeightTresh_;
00494 
00495     //try find planes with pcl::segmentation
00496     pcl::PointCloud<pcl::PointNormal>::Ptr input_surface_cloud(new pcl::PointCloud<pcl::PointNormal>);
00497     pcl::PointCloud<pcl::PointXYZI>::Ptr possible_stairsCloud(new pcl::PointCloud<pcl::PointXYZI>);
00498     pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud_debug(new pcl::PointCloud<pcl::PointXYZI>);
00499 
00500     this->getPreprocessedCloud(input_cloud, input_surface_cloud);
00501 
00502     possible_stairsCloud->header.frame_id=input_surface_cloud->header.frame_id;
00503 
00504     // try conditional clustering
00505     pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
00506     pcl::ConditionalEuclideanClustering<pcl::PointNormal> cec (false);
00507     cec.setInputCloud (input_surface_cloud);
00508     cec.setConditionFunction (&customRegionGrowing);
00509     cec.setClusterTolerance (clusterTolerance_);
00510     cec.setMinClusterSize (clusterMinSize_);
00511     cec.setMaxClusterSize (clusterMaxSize_);
00512     cec.segment (*clusters);
00513 
00514     //Clustering
00515     std::vector<Eigen::Vector3f> avg_point_per_cluster;
00516     float sum_x;
00517     float sum_y;
00518     float sum_z;
00519     float min_x=FLT_MAX;
00520     float max_x=-FLT_MAX;
00521     float min_y=FLT_MAX;
00522     float max_y=-FLT_MAX;
00523     float min_z=FLT_MAX;
00524     float max_z=-FLT_MAX;
00525     int clusterSize;
00526 
00527     int clusterColor=0;
00528     int clusterCounter=0;
00529 
00530     ROS_INFO("number cluster: %i", clusters->size ());
00531     std::vector<int> cluster_idx_corresponding_to_avg_point;
00532     for (int i = 0; i < clusters->size (); ++i){
00533         clusterSize= (*clusters)[i].indices.size ();
00534         sum_x=0;
00535         sum_y=0;
00536         sum_z=0;
00537         min_x=FLT_MAX;
00538         max_x=-FLT_MAX;
00539         min_y=FLT_MAX;
00540         max_y=-FLT_MAX;
00541         min_z=FLT_MAX;
00542         max_z=-FLT_MAX;
00543         pcl::PointNormal point;
00544         for (int j = 0; j < (*clusters)[i].indices.size (); ++j){
00545             point=input_surface_cloud->points[(*clusters)[i].indices[j]];
00546             sum_x=sum_x + point.x;
00547             sum_y=sum_y + point.y;
00548             sum_z=sum_z + point.z;
00549             if(min_x > point.x){
00550                 min_x=point.x;
00551             }
00552             if(max_x < point.x){
00553                 max_x=point.x;
00554             }
00555             if(min_y > point.y){
00556                 min_y=point.y;
00557             }
00558             if(max_y < point.y){
00559                 max_y=point.y;
00560             }
00561             if(min_z > point.z){
00562                 min_z=point.z;
00563             }
00564             if(max_z < point.z){
00565                 max_z=point.z;
00566             }
00567         }
00568 
00569         if(fabs(max_x-min_x)>maxClusterXYDimension_ || fabs(max_y-min_y)>maxClusterXYDimension_){
00570             clusterCounter=clusterCounter+1;
00571             continue;
00572         }
00573 
00574         Eigen::Vector3f avg_point;
00575         avg_point(0)=sum_x/clusterSize;
00576         avg_point(1)=sum_y/clusterSize;
00577         avg_point(2)=sum_z/clusterSize;
00578 
00579         if(fabs(max_z-min_z)/2<clusterHeightTresh){
00580             avg_point_per_cluster.push_back(avg_point);
00581             cluster_idx_corresponding_to_avg_point.push_back(clusterCounter);
00582 
00583             pcl::PointXYZI tempP;
00584             tempP.x=avg_point(0);
00585             tempP.y=avg_point(1);
00586             tempP.z=avg_point(2);
00587             tempP.intensity=clusterColor;
00588             possible_stairsCloud->points.push_back(tempP);
00589             cluster_cloud_debug->clear();
00590             cluster_cloud_debug->header.frame_id=worldFrame_;
00591             for (int j = 0; j < (*clusters)[i].indices.size (); ++j){
00592                 pcl::PointXYZI tempP;
00593                 tempP.x=input_surface_cloud->points[(*clusters)[i].indices[j]].x;
00594                 tempP.y=input_surface_cloud->points[(*clusters)[i].indices[j]].y;
00595                 tempP.z=input_surface_cloud->points[(*clusters)[i].indices[j]].z;
00596                 tempP.intensity=clusterColor;
00597                 possible_stairsCloud->points.push_back(tempP);
00598                 cluster_cloud_debug->points.push_back(tempP);
00599             }
00600             clusterColor=clusterColor+1;
00601         }
00602         clusterCounter=clusterCounter+1;
00603     }
00604 
00605     if(possible_stairs_cloud_pub_.getNumSubscribers()>0){
00606         possible_stairs_cloud_pub_.publish(possible_stairsCloud);
00607     }
00608     ROS_INFO("number final cluster: %i", avg_point_per_cluster.size());
00609 
00610     if(avg_point_per_cluster.size()>=2){
00611 
00612         //jeder punkt mit jedem Grade bilden und Abstände berechnen
00613         int minCountPointsAtLine=minRequiredPointsOnLine_; //number points to stay at line
00614         int pointsAtLineCounter=0;
00615         float distanceToLineThreshold=distanceToLineTresh_;
00616         Eigen::Vector3f direction;
00617         Eigen::Vector3f base;
00618         Eigen::Vector3f point;
00619         std::vector<int> point_in_line_counter;
00620         point_in_line_counter.resize(avg_point_per_cluster.size());
00621         for(int i=0; i< point_in_line_counter.size(); i++){
00622             point_in_line_counter.at(i)=0;
00623         }
00624 
00625 
00626         int maxPointOnLineCounter=0;
00627         int best_pair_idx1;
00628         int best_pair_idx2;
00629         pcl::PointCloud<pcl::PointXYZI>::Ptr debug_line_cloud(new pcl::PointCloud<pcl::PointXYZI>());
00630         if(avg_point_per_cluster.size() != 0){
00631             for(int i=0; i<avg_point_per_cluster.size()-1; i++){
00632                 for(int j=i+1; j<avg_point_per_cluster.size(); j++){
00633                     debug_line_cloud->clear();
00634                     debug_line_cloud->header.frame_id=input_surface_cloud->header.frame_id;
00635 
00636                     base(0)= avg_point_per_cluster.at(i)(0);
00637                     base(1)= avg_point_per_cluster.at(i)(1);
00638                     base(2)= avg_point_per_cluster.at(i)(2);
00639 
00640                     direction(0)= avg_point_per_cluster.at(j)(0) - base(0);
00641                     direction(1)= avg_point_per_cluster.at(j)(1) - base(1);
00642                     direction(2)= avg_point_per_cluster.at(j)(2) - base(2);
00643 
00644                     pcl::PointXYZI pushbackPoint;
00645                     pushbackPoint.x=base(0);
00646                     pushbackPoint.y=base(1);
00647                     pushbackPoint.z=base(2);
00648                     pushbackPoint.intensity=10;
00649                     debug_line_cloud->push_back(pushbackPoint);
00650                     pushbackPoint.x=direction(0)+base(0);
00651                     pushbackPoint.y=direction(1)+base(1);
00652                     pushbackPoint.z=direction(2)+base(2);
00653                     pushbackPoint.intensity=10;
00654                     debug_line_cloud->push_back(pushbackPoint);
00655 
00656                     pointsAtLineCounter=0;
00657                     for(int p=0; p<avg_point_per_cluster.size(); p++){
00658                         point(0)=avg_point_per_cluster.at(p)(0);
00659                         point(1)=avg_point_per_cluster.at(p)(1);
00660                         point(2)=avg_point_per_cluster.at(p)(2);
00661 
00662                         float tempA= std::sqrt(direction.cross(point-base).dot(direction.cross(point-base)));
00663                         float lengthDirection= std::sqrt(direction.dot(direction));
00664                         float distance= tempA/lengthDirection;
00665 
00666                         if(distance <= distanceToLineThreshold){
00667                             pointsAtLineCounter=pointsAtLineCounter+1;
00668                             pushbackPoint.x=point(0);
00669                             pushbackPoint.y=point(1);
00670                             pushbackPoint.z=point(2);
00671                             pushbackPoint.intensity=distance;
00672                             debug_line_cloud->push_back(pushbackPoint);
00673 
00674                             point_in_line_counter.at(p)=point_in_line_counter.at(p)+1;
00675                         }
00676                     }
00677 
00678                     if(pointsAtLineCounter >= maxPointOnLineCounter){
00679                         maxPointOnLineCounter=pointsAtLineCounter;
00680                         best_pair_idx1=i;
00681                         best_pair_idx2=j;
00682                     }
00683 
00684 
00685 
00686                 }
00687             }
00688         }
00689 
00690         int iterationCounter=0;
00691         while(1){
00692             //get best pair index depending on iteration
00693             maxPointOnLineCounter=0;
00694             best_pair_idx1=-1;
00695             best_pair_idx2=-1;
00696             for(int p=0; p<point_in_line_counter.size(); p++){
00697                 if(point_in_line_counter.at(p) > maxPointOnLineCounter){
00698                     maxPointOnLineCounter=point_in_line_counter.at(p);
00699                     best_pair_idx2=best_pair_idx1;
00700                     best_pair_idx1=p;
00701                 }
00702             }
00703 
00704             if(best_pair_idx1==-1 || best_pair_idx2==-1){
00705                 break;
00706             }
00707 
00708             point_in_line_counter.at(best_pair_idx1)=0;
00709 
00710             //construct line form most used points, middel is the position of the stairs
00711 
00712             base=avg_point_per_cluster.at(best_pair_idx1);
00713 
00714             direction(0)= avg_point_per_cluster.at(best_pair_idx2)(0) - base(0);
00715             direction(1)= avg_point_per_cluster.at(best_pair_idx2)(1) - base(1);
00716             direction(2)= avg_point_per_cluster.at(best_pair_idx2)(2) - base(2);
00717 
00718             pcl::PointXYZI pushbackPoint;
00719             debug_line_cloud->clear();
00720             debug_line_cloud->header.frame_id=input_cloud->header.frame_id;
00721             pushbackPoint.x=base(0);
00722             pushbackPoint.y=base(1);
00723             pushbackPoint.z=base(2);
00724 
00725             debug_line_cloud->push_back(pushbackPoint);
00726             pushbackPoint.x=avg_point_per_cluster.at(best_pair_idx2)(0);
00727             pushbackPoint.y=avg_point_per_cluster.at(best_pair_idx2)(1);
00728             pushbackPoint.z=avg_point_per_cluster.at(best_pair_idx2)(2);
00729 
00730             debug_line_cloud->push_back(pushbackPoint);
00731 
00732             pointsAtLineCounter=2;
00733             std::vector<int> final_cluster_idx;
00734             for(int p=0; p<avg_point_per_cluster.size(); p++){
00735                 if(p==best_pair_idx1 || p== best_pair_idx2) {
00736                     final_cluster_idx.push_back(cluster_idx_corresponding_to_avg_point.at(p));
00737                     continue;
00738                 }
00739                 pushbackPoint.x=point(0)=avg_point_per_cluster.at(p)(0);
00740                 pushbackPoint.y=point(1)=avg_point_per_cluster.at(p)(1);
00741                 pushbackPoint.z=point(2)=avg_point_per_cluster.at(p)(2);
00742 
00743 
00744                 float tempA= std::sqrt(direction.cross(point-base).dot(direction.cross(point-base)));
00745                 float lengthDirection= std::sqrt(direction.dot(direction));
00746                 float distance= tempA/lengthDirection;
00747 
00748                 if(distance <= distanceToLineThreshold){
00749                     pointsAtLineCounter=pointsAtLineCounter+1;
00750                     final_cluster_idx.push_back(cluster_idx_corresponding_to_avg_point.at(p));
00751                     debug_line_cloud->push_back(pushbackPoint);
00752 
00753                 }
00754             }
00755             points_on_line_cloud_debug_.publish(debug_line_cloud);
00756             if(pointsAtLineCounter >= minCountPointsAtLine){
00757                 //publish results
00758                 if(points_on_line_cloud_debug_.getNumSubscribers()>0){
00759                     points_on_line_cloud_debug_.publish(debug_line_cloud);
00760                 }
00761                 if(maxDistBetweenPoints(debug_line_cloud) <= maxDistBetweenStairsPoints_ && minHightDistBetweenPoints(debug_line_cloud) >= minHightDistBetweenAllStairsPoints_){
00762                     ROS_INFO("Staris; number points on line: %i", pointsAtLineCounter);
00763 
00764                     Eigen::Vector3f dir;
00765                     if(avg_point_per_cluster.at(best_pair_idx2)(2) >=base(2)){
00766                         dir=avg_point_per_cluster.at(best_pair_idx2)-base;
00767                     }else{
00768                         dir=base-avg_point_per_cluster.at(best_pair_idx2);
00769                     }
00770                     pcl::PointCloud<pcl::PointXYZ>::Ptr planeCloud(new pcl::PointCloud<pcl::PointXYZ>());
00771                     //is not needed while detecting stairs on unfilterd scancloud
00772 //                                        stairsSreachPlaneDetection(input_surface_cloud, debug_line_cloud, base, dir, planeCloud);
00773                     publishResults(input_surface_cloud, planeCloud, clusters, final_cluster_idx, base, direction+base);
00774                 }else{
00775                     ROS_INFO("No stairs, distance between points to large, or heightDistance between point too small");
00776                 }
00777 
00778             }else{
00779                 ROS_INFO("No stairs");
00780             }
00781 //            sleep(2);
00782             iterationCounter=iterationCounter+1;
00783         }
00784     }
00785     ROS_INFO("No more possible stairs");
00786 
00787 }
00788 
00789 void HectorStairDetection::stairsSreachPlaneDetection(pcl::PointCloud<pcl::PointNormal>::Ptr &input_surface_cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr points_on_line, Eigen::Vector3f base, Eigen::Vector3f dir, pcl::PointCloud<pcl::PointXYZ>::Ptr &planeCloud){
00790     ROS_INFO("run plane segmentation");
00791 
00792     Eigen::Vector3f temp=dir.cross(Eigen::Vector3f(0,0,1));
00793     Eigen::Vector3f searchAxes=dir.cross(temp);
00794     searchAxes=searchAxes/searchAxes.squaredNorm();
00795 
00796     visualization_msgs::Marker line_list;
00797     line_list.header.frame_id=worldFrame_;
00798     line_list.id = 42;
00799     line_list.type = visualization_msgs::Marker::LINE_LIST;
00800     line_list.scale.x = 0.1;
00801     line_list.color.r = 1.0;
00802     line_list.color.a = 1.0;
00803 
00804     geometry_msgs::Point p;
00805     p.x = base(0);
00806     p.y = base(1);
00807     p.z = base(2);
00808 
00809     line_list.points.push_back(p);
00810     p.x = base(0) + searchAxes(0);
00811     p.y = base(1) + searchAxes(1);
00812     p.z = base(2) + searchAxes(2);
00813     line_list.points.push_back(p);
00814 
00815     line_marker_pub_.publish(line_list);
00816 
00817     pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>());
00818     searchCloud->resize(input_surface_cloud->size());
00819     searchCloud->header.frame_id=worldFrame_;
00820     for (size_t i = 0; i < input_surface_cloud->points.size(); ++i)
00821     {
00822         const pcl::PointNormal &mls_pt = input_surface_cloud->points[i];
00823         pcl::PointXYZ pt(mls_pt.x, mls_pt.y, mls_pt.z);
00824         searchCloud->push_back(pt);
00825     }
00826 
00827     //search just in stairs environment
00828     Eigen::Vector2f xAxes;
00829     xAxes(0)=1;
00830     xAxes(1)=0;
00831     Eigen::Vector2f dir2f;
00832     dir2f(0)=dir(0);
00833     dir2f(1)=dir(1);
00834     float angleXToStairs=acos((dir2f.dot(xAxes))/(dir2f.norm()*xAxes.norm()));
00835 
00836     pcl::PointCloud<pcl::PointXYZ>::Ptr processCloud_v1(new pcl::PointCloud<pcl::PointXYZ>());
00837     pcl::PointCloud<pcl::PointXYZ>::Ptr processCloud_v2(new pcl::PointCloud<pcl::PointXYZ>());
00838     pcl::PassThrough<pcl::PointXYZ> pass;
00839     pass.setInputCloud(searchCloud);
00840     pass.setFilterFieldName("z");
00841     pass.setFilterLimits(0.2, passThroughZMax_);
00842     pass.filter(*processCloud_v1);
00843 
00844     pass.setInputCloud(processCloud_v1);
00845     pass.setFilterFieldName("y");
00846     pass.setFilterLimits(base(1)-fabs(sin(angleXToStairs)*maxDistBetweenStairsPoints_)-maxClusterXYDimension_/2, base(1)+fabs(sin(angleXToStairs)*maxDistBetweenStairsPoints_)+maxClusterXYDimension_/2);
00847     pass.filter(*processCloud_v2);
00848 
00849     pass.setInputCloud(processCloud_v2);
00850     pass.setFilterFieldName("x");
00851     pass.setFilterLimits(base(0)-fabs(cos(angleXToStairs)*maxDistBetweenStairsPoints_)-maxClusterXYDimension_/2, base(0)+fabs(cos(angleXToStairs)*maxDistBetweenStairsPoints_)+maxClusterXYDimension_/2);
00852     pass.filter(*searchCloud);
00853 
00854     temp_after_pass_trough_pub_.publish(searchCloud);
00855 
00856     planeCloud->header.frame_id=worldFrame_;
00857     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00858     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00859     // Create the segmentation object
00860     pcl::SACSegmentation<pcl::PointXYZ> seg;
00861     seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00862     seg.setMethodType (pcl::SAC_RANSAC);
00863     seg.setDistanceThreshold (planeSegDistTresh_);
00864     seg.setAxis(searchAxes);
00865     seg.setEpsAngle(planeSegAngleEps_);
00866 
00867     seg.setInputCloud (searchCloud);
00868     seg.segment (*inliers, *coefficients);
00869 
00870     if (inliers->indices.size () == 0)
00871     {
00872         PCL_ERROR ("Could not estimate more planar models for the given dataset.");
00873     }
00874 
00875     ROS_DEBUG("extract plane and rest potins");
00876     pcl::ExtractIndices<pcl::PointXYZ> extract;
00877     extract.setInputCloud (searchCloud);
00878     extract.setIndices (inliers);
00879     extract.setNegative(false);
00880     extract.filter (*planeCloud);
00881     planeCloud->header.frame_id=worldFrame_;
00882 
00883     //check if plane contains stairs points
00884     Eigen::Vector3f normal;
00885     normal(0)=coefficients->values[0];
00886     normal(1)=coefficients->values[1];
00887     normal(2)=coefficients->values[2];
00888 
00889     Eigen::Vector3f point;
00890     point(0)=0;
00891     point(1)=0;
00892     point(2)=-(coefficients->values[3]/coefficients->values[2]);
00893 
00894     Eigen::Vector3f normal_0;
00895     normal_0= normal/normal.squaredNorm();
00896 
00897     float d_hesse= point.dot(normal_0);
00898 
00899     bool isPossiblePlane=true;
00900     Eigen::Vector3f stairs_point;
00901     for(int i=0; i<points_on_line->size(); i++){
00902         stairs_point(0)=points_on_line->at(i).x;
00903         stairs_point(1)=points_on_line->at(i).y;
00904         stairs_point(2)=points_on_line->at(i).z;
00905         //        std::cout<<"hesse distance: "<<fabs(stairs_point.dot(normal_0)-d_hesse) <<std::endl;
00906         if(fabs(stairs_point.dot(normal_0)-d_hesse) > hesseTresh_){
00907             isPossiblePlane=false;
00908             break;
00909         }
00910     }
00911 
00912     if(isPossiblePlane){
00913         ROS_INFO("staris plane found");
00914         cloud_after_plane_detection_debug_pub_.publish(planeCloud);
00915     }else{
00916         planeCloud->resize(0);
00917     }
00918 }
00919 
00920 float HectorStairDetection::maxDistBetweenPoints(pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud){
00921     float max_dist=0;
00922     float temp_dist=0;
00923     for(int i=0; i<input_cloud->size(); i++){
00924         for(int j=i; j<input_cloud->size(); j++){
00925             temp_dist= std::sqrt(std::pow(input_cloud->at(i).x-input_cloud->at(j).x, 2)+std::pow(input_cloud->at(i).y-input_cloud->at(j).y, 2)+std::pow(input_cloud->at(i).z-input_cloud->at(j).z, 2));
00926             if(temp_dist>max_dist){
00927                 max_dist=temp_dist;
00928             }
00929         }
00930     }
00931     return max_dist;
00932 }
00933 
00934 float HectorStairDetection::minHightDistBetweenPoints(pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud){
00935     float min_dist=FLT_MAX;
00936     float temp_dist=0;
00937     for(int i=0; i<input_cloud->size(); i++){
00938         for(int j=i+1; j<input_cloud->size(); j++){
00939             temp_dist= fabs(input_cloud->at(i).z-input_cloud->at(j).z);
00940             if(temp_dist<min_dist){
00941                 min_dist=temp_dist;
00942             }
00943         }
00944     }
00945     return min_dist;
00946 }
00947 
00948 }
00949 
00950 


hector_stair_detection
Author(s):
autogenerated on Thu Jun 6 2019 22:10:09