footstep_state.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_footstep_planner/footstep_state.h"
00037 
00038 #include <eigen_conversions/eigen_msg.h>
00039 
00040 #include <jsk_recognition_utils/pcl_conversion_util.h>
00041 #include <jsk_recognition_utils/geo_util.h>
00042 #include "jsk_footstep_planner/line2d.h"
00043 
00044 #define DEBUG 0
00045 #if DEBUG
00046 // debug
00047 #include <ros/ros.h>
00048 #include <visualization_msgs/Marker.h>
00049 #include <visualization_msgs/MarkerArray.h>
00050 #endif
00051 
00052 #define DEBUG_PRINT(proc) if (debug_print_) { std::cerr << proc << std::endl; }
00053 
00054 namespace jsk_footstep_planner
00055 {
00056 #if DEBUG
00057   ros::Publisher pub_debug_marker;
00058 #endif
00059   std::string projectStateToString(unsigned int state)
00060   {
00061     if (state == projection_state::success) {
00062       return "success";
00063     }
00064     else if (state == projection_state::no_pointcloud) {
00065       return "no pointcloud";
00066     }
00067     else if (state == projection_state::no_enough_support) {
00068       return "no enough support";
00069     }
00070     else if (state == projection_state::no_plane) {
00071       return "no plane";
00072     }
00073     else if (state == projection_state::no_enough_inliers) {
00074       return "no enough inliers";
00075     }
00076     else if (state == projection_state::close_to_success) {
00077       return "close to success";
00078     }
00079     else {
00080       return "unknown error";
00081     }
00082   }
00083   
00084   jsk_footstep_msgs::Footstep::Ptr
00085   FootstepState::toROSMsg()
00086   {
00087     jsk_footstep_msgs::Footstep::Ptr ret(new jsk_footstep_msgs::Footstep);
00088     tf::poseEigenToMsg(pose_, ret->pose);
00089     ret->dimensions.x = dimensions_[0];
00090     ret->dimensions.y = dimensions_[1];
00091     ret->dimensions.z = dimensions_[2];
00092     ret->leg = leg_;
00093     return ret;
00094   }
00095   jsk_footstep_msgs::Footstep::Ptr
00096   FootstepState::toROSMsg(const Eigen::Vector3f& ioffset)
00097   {
00098     jsk_footstep_msgs::Footstep::Ptr ret(new jsk_footstep_msgs::Footstep);
00099     Eigen::Affine3f newpose = pose_ * Eigen::Translation3f(ioffset);
00100     tf::poseEigenToMsg(newpose, ret->pose);
00101     ret->dimensions.x = dimensions_[0];
00102     ret->dimensions.y = dimensions_[1];
00103     ret->dimensions.z = dimensions_[2];
00104     ret->offset.x = - ioffset[0];
00105     ret->offset.y = - ioffset[1];
00106     ret->offset.z = - ioffset[2];
00107     ret->leg = leg_;
00108     return ret;
00109   }
00110 
00111   pcl::PointIndices::Ptr
00112   FootstepState::cropPointCloudExact(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00113                                      pcl::PointIndices::Ptr near_indices,
00114                                      double padding_x, double padding_y)
00115   {
00116     // Project vertices into 2d
00117     Eigen::Vector3f z(0, 0, 1);
00118     Eigen::Vector3f a = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x,  dimensions_[1]/2 + padding_y, 0);
00119     Eigen::Vector3f b = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x,  dimensions_[1]/2 + padding_y, 0);
00120     Eigen::Vector3f c = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, -dimensions_[1]/2 - padding_y, 0);
00121     Eigen::Vector3f d = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, -dimensions_[1]/2 - padding_y, 0);
00122     Eigen::Vector3f a_2d = a + (- z.dot(a)) * z;
00123     Eigen::Vector3f b_2d = b + (- z.dot(b)) * z;
00124     Eigen::Vector3f c_2d = c + (- z.dot(c)) * z;
00125     Eigen::Vector3f d_2d = d + (- z.dot(d)) * z;
00126 
00127     Eigen::Vector2f a2d(a_2d[0], a_2d[1]);
00128     Eigen::Vector2f b2d(b_2d[0], b_2d[1]);
00129     Eigen::Vector2f c2d(c_2d[0], c_2d[1]);
00130     Eigen::Vector2f d2d(d_2d[0], d_2d[1]);
00131     //std::set<int> set_indices;
00132     pcl::PointIndices::Ptr ret(new pcl::PointIndices);
00133     ret->indices.reserve(near_indices->indices.size());
00134     const std::vector<int> near_indices_indices = near_indices->indices;
00135     for (size_t i = 0; i < near_indices->indices.size(); i++) {
00136       const int index = near_indices_indices[i];
00137       const pcl::PointNormal point = cloud->points[index];
00138       const Eigen::Vector3f ep = point.getVector3fMap();
00139       const Eigen::Vector3f point_2d = ep + (-z.dot(ep)) * z;
00140       const Eigen::Vector2f p2d(point_2d[0], point_2d[1]);
00141       if (cross2d((b2d - a2d), (p2d - a2d)) > 0 &&
00142           cross2d((c2d - b2d), (p2d - b2d)) > 0 &&
00143           cross2d((d2d - c2d), (p2d - c2d)) > 0 &&
00144           cross2d((a2d - d2d), (p2d - d2d)) > 0) {
00145         //set_indices.insert(index);
00146         ret->indices.push_back(index);
00147       }
00148     }
00149 
00150     //ret->indices = std::vector<int>(set_indices.begin(), set_indices.end());
00151     return ret;
00152   }
00153   
00154   pcl::PointIndices::Ptr
00155   FootstepState::cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00156                                 ANNGrid::Ptr grid_search,
00157                                 double padding_x, double padding_y)
00158   {
00159     pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
00160     Eigen::Vector3f a = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x,  dimensions_[1]/2 + padding_y, 0);
00161     Eigen::Vector3f b = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x,  dimensions_[1]/2 + padding_y, 0);
00162     Eigen::Vector3f c = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, -dimensions_[1]/2 - padding_y, 0);
00163     Eigen::Vector3f d = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, -dimensions_[1]/2 - padding_y, 0);
00164     grid_search->approximateSearchInBox(a, b, c, d, *near_indices);
00165     return cropPointCloudExact(cloud, near_indices, padding_x, padding_y);
00166   }
00167   
00168 #if 0
00169   pcl::PointIndices::Ptr
00170   FootstepState::cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00171                                 pcl::search::Octree<pcl::PointNormal>& tree)
00172   {
00173     pcl::PointNormal center;
00174     center.getVector3fMap() = Eigen::Vector3f(pose_.translation());
00175     center.z = 0.0;
00176     float r = 0.2;
00177     pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
00178     std::vector<float> distances;
00179     tree.radiusSearch(center, r, near_indices->indices, distances);
00180     return cropPointCloudExact(cloud, near_indices);
00181   }
00182 #endif
00183   bool FootstepState::crossCheck(FootstepState::Ptr other, float collision_padding)
00184   {
00185     Eigen::Vector3f a0, a1, a2, a3;
00186     Eigen::Vector3f b0, b1, b2, b3;
00187     vertices(a0, a1, a2, a3, collision_padding);
00188     other->vertices(b0, b1, b2, b3, collision_padding);
00189     Line2D a_01(a0, a1), a_12(a1, a2), a_23(a2, a3), a_30(a3, a0);
00190     Line2D b_01(b0, b1), b_12(b1, b2), b_23(b2, b3), b_30(b3, b0);
00191     return !(a_01.isCrossing(b_01) ||
00192              a_01.isCrossing(b_12) ||
00193              a_01.isCrossing(b_23) ||
00194              a_01.isCrossing(b_30) ||
00195              a_12.isCrossing(b_01) ||
00196              a_12.isCrossing(b_12) ||
00197              a_12.isCrossing(b_23) ||
00198              a_12.isCrossing(b_30) ||
00199              a_23.isCrossing(b_01) ||
00200              a_23.isCrossing(b_12) ||
00201              a_23.isCrossing(b_23) ||
00202              a_23.isCrossing(b_30) ||
00203              a_30.isCrossing(b_01) ||
00204              a_30.isCrossing(b_12) ||
00205              a_30.isCrossing(b_23) ||
00206              a_30.isCrossing(b_30));
00207   }
00208   
00209   FootstepState::Ptr
00210   FootstepState::projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00211                                 pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00212                                 ANNGrid::Ptr grid_search,
00213                                 pcl::search::Octree<pcl::PointNormal>& tree_2d,
00214                                 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
00215                                 const Eigen::Vector3f& z,
00216                                 unsigned int& error_state,
00217                                 FootstepParameters &parameters)
00218   {
00219     // TODO: z is ignored
00220     // extract candidate points
00221     //pcl::PointIndices::Ptr indices = cropPointCloud(cloud_2d, tree_2d);
00222     // Before computing, check is it supported or not to omit recognition
00223     DEBUG_PRINT(std::endl << "[FS state] projectToCloud");
00224     pcl::PointIndices::Ptr indices;
00225     FootstepSupportState presupport_state;
00226     if (parameters.skip_cropping) {
00227       presupport_state = isSupportedByPointCloudWithoutCropping(
00228         pose_, cloud, tree, indices,
00229         parameters.support_check_x_sampling,
00230         parameters.support_check_y_sampling,
00231         parameters.support_check_vertex_neighbor_threshold);
00232       DEBUG_PRINT("[FS state] pre /(skip_cropping) projection state " << presupport_state);
00233     }
00234     indices = cropPointCloud(cloud, grid_search,
00235                              parameters.support_padding_x,
00236                              parameters.support_padding_y);
00237     DEBUG_PRINT("[FS state] pre / indices " << indices->indices.size());
00238     if (indices->indices.size() < parameters.plane_estimation_min_inliers) {
00239       DEBUG_PRINT("[FS state] no enough inliners");
00240       error_state = projection_state::no_enough_inliers;
00241       return FootstepState::Ptr();
00242     }
00243     if (!parameters.skip_cropping) {
00244 #if DEBUG
00245       double ax = 0.0, ay = 0.0, az = 0.0;
00246       double xx = 0.0, yy = 0.0, zz = 0.0;
00247       for (size_t i = 0; i < indices->indices.size(); i++) {
00248         pcl::PointNormal pp = cloud->points[indices->indices[i]];
00249         ROS_INFO("%d %f %f %f", indices->indices[i], pp.x, pp.y, pp.z);
00250         ax += pp.x; ay += pp.y; az += pp.z;
00251         xx += pp.x*pp.x; yy += pp.y*pp.y; zz += pp.z*pp.z;
00252       }
00253       int ss = indices->indices.size();
00254       ROS_INFO("ave( %d ): %f %f %f, %f %f %f",
00255                ss, ax/ss, ay/ss, az/ss,
00256                sqrt(xx/ss - (ax/ss)*(ax/ss)),
00257                sqrt(yy/ss - (ay/ss)*(ay/ss)),
00258                sqrt(zz/ss - (az/ss)*(az/ss)));
00259 #endif
00260       presupport_state = isSupportedByPointCloud(
00261         pose_, cloud, tree, indices,
00262         parameters.support_check_x_sampling,
00263         parameters.support_check_y_sampling,
00264         parameters.support_check_vertex_neighbor_threshold);
00265       DEBUG_PRINT("[FS state] pre / (!skip_cropping) projection state " << presupport_state);
00266     }
00267     if (presupport_state == projection_state::success) {
00268       return FootstepState::Ptr(new FootstepState(leg_, pose_, dimensions_,
00269                                                   resolution_,
00270                                                   index_x_,
00271                                                   index_y_,
00272                                                   index_yaw_));
00273     }
00274     // estimate plane with ransac
00275     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00276     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00277     if (!parameters.plane_estimation_use_normal) {
00278       pcl::SACSegmentation<pcl::PointNormal> seg;
00279       seg.setOptimizeCoefficients (true);
00280       seg.setRadiusLimits(0.01, std::numeric_limits<double>::max ());
00281       seg.setMethodType(pcl::SAC_RANSAC);
00282       seg.setDistanceThreshold(parameters.plane_estimation_outlier_threshold);
00283       seg.setModelType(pcl::SACMODEL_PLANE);
00284       seg.setInputCloud(cloud);
00285       //
00286       seg.setIndices(indices);
00287       seg.setMaxIterations(parameters.plane_estimation_max_iterations);
00288       seg.segment(*inliers, *coefficients);
00289     } else {
00290       pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg;
00291       seg.setOptimizeCoefficients (true);
00292       seg.setRadiusLimits(0.01, std::numeric_limits<double>::max ());
00293       seg.setMethodType(pcl::SAC_RANSAC);
00294       seg.setDistanceThreshold(parameters.plane_estimation_outlier_threshold);
00295       seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
00296       seg.setInputCloud(cloud);
00297       //
00298       seg.setInputNormals(cloud);
00299       seg.setNormalDistanceWeight(parameters.plane_estimation_normal_distance_weight);
00300       seg.setMinMaxOpeningAngle(-parameters.plane_estimation_normal_opening_angle,
00301                                 parameters.plane_estimation_normal_opening_angle);
00302       //
00303       seg.setIndices(indices);
00304       seg.setMaxIterations(parameters.plane_estimation_max_iterations);
00305       seg.segment(*inliers, *coefficients);
00306     }
00307 
00308     DEBUG_PRINT( "[FS state] inliers " << inliers->indices.size() );
00309     if (inliers->indices.size() == 0) {
00310       DEBUG_PRINT( "[FS state] no plane" );
00311       error_state = projection_state::no_plane;
00312       return FootstepState::Ptr();
00313     }
00314     else if (inliers->indices.size() < parameters.plane_estimation_min_inliers) {
00315       DEBUG_PRINT( "[FS state] no enough inliners " << inliers->indices.size() );
00316       error_state = projection_state::no_enough_inliers;
00317       return FootstepState::Ptr();
00318     }
00319     else {
00320       jsk_recognition_utils::Plane plane(coefficients->values);
00321       if (!plane.isSameDirection(z)) {
00322         plane = plane.flip();
00323       }
00324       // plane
00325       // DEBUG_PRINT( "[FS state] no enough inliners" );
00326       Eigen::Vector3f n = plane.getNormal();
00327       Eigen::Vector3f x = pose_.matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX();
00328       if (acos(n.dot(x)) == 0) {
00329         error_state = projection_state::vertical_footstep;
00330         return FootstepState::Ptr();
00331       }
00332       Eigen::Vector3f rotation_axis = n.cross(x).normalized();
00333       Eigen::Vector3f new_x = Eigen::AngleAxisf(M_PI / 2.0, rotation_axis) * n;
00334       if (acos(new_x.dot(x)) > M_PI / 2.0) {
00335         new_x = - new_x;
00336       }
00337       Eigen::Vector3f new_y = n.cross(new_x);
00338       Eigen::Matrix3f new_rot_mat;
00339       new_rot_mat << new_x, new_y, n;
00340       Eigen::Quaternionf new_rot(new_rot_mat);
00341       Eigen::Vector3f p(pose_.translation());
00342       double alpha = (- plane.getD() - n.dot(p)) / (n.dot(z));
00343       Eigen::Vector3f q = p + alpha * z;
00344       
00345       Eigen::Affine3f new_pose = Eigen::Translation3f(q) * new_rot;
00346       // std::cout << "new_pose: " << std::endl << new_pose.matrix() << std::endl;
00347       // std::cout << "q: " << std::endl << q << std::endl;
00348       // std::cout << "new_rot_mat: " << std::endl << new_rot_mat << std::endl;
00349       //Eigen::Affine3f new_pose = new_rot * Eigen::Translation3f(q);
00350       // check is it enough points to support the footstep
00351 #if DEBUG
00352       visualization_msgs::Marker marker;
00353       marker.header.frame_id = "map";
00354       marker.header.stamp = ros::Time();
00355       //marker.ns = "my_namespace";
00356       marker.id = 0;
00357       marker.type = visualization_msgs::Marker::POINTS;
00358       marker.action = visualization_msgs::Marker::ADD;
00359       marker.pose.position.x = 0;
00360       marker.pose.position.y = 0;
00361       marker.pose.position.z = 0;
00362       marker.pose.orientation.x = 0.0;
00363       marker.pose.orientation.y = 0.0;
00364       marker.pose.orientation.z = 0.0;
00365       marker.pose.orientation.w = 1.0;
00366       marker.scale.x = 0.01;
00367       marker.scale.y = 0.01;
00368       marker.scale.z = 0.1;
00369       marker.color.a = 1.0; // Don't forget to set the alpha!
00370       marker.color.r = 1.0;
00371       marker.color.g = 0.0;
00372       marker.color.b = 0.0;
00373       //marker.points.resize(inliers->indices.size());
00374       for(int i; i < inliers->indices.size(); i++) {
00375         geometry_msgs::Point pp;
00376         pcl::PointNormal pt = cloud->points[inliers->indices[i]];
00377         pp.x = pt.x;
00378         pp.y = pt.y;
00379         pp.z = pt.z;
00380         marker.points.push_back(pp);
00381       }
00382       visualization_msgs::Marker marker_p;
00383       marker_p.header.frame_id = "map";
00384       marker_p.header.stamp = ros::Time();
00385       //marker_p.ns = "my_namespace";
00386       marker_p.id = 1;
00387       marker_p.type = visualization_msgs::Marker::POINTS;
00388       marker_p.action = visualization_msgs::Marker::ADD;
00389       marker_p.pose.position.x = 0;
00390       marker_p.pose.position.y = 0;
00391       marker_p.pose.position.z = 0;
00392       marker_p.pose.orientation.x = 0.0;
00393       marker_p.pose.orientation.y = 0.0;
00394       marker_p.pose.orientation.z = 0.0;
00395       marker_p.pose.orientation.w = 1.0;
00396       marker_p.scale.x = 0.01;
00397       marker_p.scale.y = 0.01;
00398       marker_p.scale.z = 0.1;
00399       marker_p.color.a = 1.0; // Don't forget to set the alpha!
00400       marker_p.color.r = 0.0;
00401       marker_p.color.g = 0.0;
00402       marker_p.color.b = 1.0;
00403       //marker.points.resize(inliers->indices.size());
00404       for(int i; i < inliers->indices.size(); i++) {
00405         geometry_msgs::Point pp;
00406         pcl::PointNormal pt = cloud->points[inliers->indices[i]];
00407         Eigen::Vector3f ep(pt.x, pt.y, pt.z);
00408         Eigen::Vector3f rt;
00409         plane.project(ep, rt);
00410         pp.x = rt(0);
00411         pp.y = rt(1);
00412         pp.z = rt(2);
00413         marker_p.points.push_back(pp);
00414       }
00415 
00416       //only if using a MESH_RESOURCE marker type:
00417       visualization_msgs::MarkerArray arry;
00418       arry.markers.push_back(marker);
00419       arry.markers.push_back(marker_p);
00420       pub_debug_marker.publish( arry );
00421 #endif
00422       FootstepSupportState support_state;
00423       if (parameters.skip_cropping) {
00424         support_state = isSupportedByPointCloudWithoutCropping(
00425           new_pose, cloud, tree, inliers,
00426           parameters.support_check_x_sampling,
00427           parameters.support_check_y_sampling,
00428           parameters.support_check_vertex_neighbor_threshold);
00429         DEBUG_PRINT( "[FS state] (skip_cropping) projection state " << support_state );
00430       }
00431       else {
00432         support_state = isSupportedByPointCloud(
00433           new_pose, cloud, tree, inliers,
00434           parameters.support_check_x_sampling,
00435           parameters.support_check_y_sampling,
00436           parameters.support_check_vertex_neighbor_threshold);
00437         DEBUG_PRINT( "[FS state] (!skip_cropping) projection state " << support_state );
00438       }
00439       if (support_state == NOT_SUPPORTED) {
00440         DEBUG_PRINT( "[FS state] NOT SUPPORTED" );
00441         error_state = projection_state::no_enough_support;
00442         return FootstepState::Ptr();
00443       }
00444       else if (support_state == CLOSE_TO_SUPPORTED) {
00445         DEBUG_PRINT( "[FS state] CLOSE TO SUPPORTED" );
00446         error_state = projection_state::close_to_success;
00447         return FootstepState::Ptr();
00448       }
00449       else if ((inliers->indices.size() / (double)indices->indices.size()) < parameters.plane_estimation_min_ratio_of_inliers ) {
00450         DEBUG_PRINT( "[FS state] ratio of inliers " << (inliers->indices.size() / (double)indices->indices.size()) );
00451         error_state = projection_state::no_enough_inliers_ratio;
00452         return FootstepState::Ptr();
00453       }
00454       else {
00455         DEBUG_PRINT( "[FS state] success" );
00456         error_state = projection_state::success;
00457         return FootstepState::Ptr(new FootstepState(leg_, new_pose, dimensions_,
00458                                                     resolution_,
00459                                                     index_x_,
00460                                                     index_y_,
00461                                                     index_yaw_));
00462       }
00463     }
00464   }
00465   
00466   FootstepSupportState
00467   FootstepState::isSupportedByPointCloud(const Eigen::Affine3f& pose,
00468                                          pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00469                                          pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00470                                          pcl::PointIndices::Ptr inliers,
00471                                          const int foot_x_sampling_num,
00472                                          const int foot_y_sampling_num,
00473                                          const double vertex_threshold)
00474   {
00475     const double dx = dimensions_[0] / foot_x_sampling_num;
00476     const double dy = dimensions_[1] / foot_y_sampling_num;
00477     // vertices
00478     const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
00479     const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
00480     const Eigen::Affine3f new_origin = pose *
00481       Eigen::Translation3f(- ux * dimensions_[0] / 2.0) *
00482       Eigen::Translation3f(- uy * dimensions_[1] / 2.0);
00483     const Eigen::Affine3f inv_pose = new_origin.inverse();
00484     
00485     bool occupiedp[foot_x_sampling_num][foot_y_sampling_num];
00486     // Initialize by false
00487     for (size_t i = 0; i < foot_x_sampling_num; i++) {
00488       for (size_t j = 0; j < foot_y_sampling_num; j++) {
00489         occupiedp[i][j] = false;
00490       }
00491     }
00492     
00493     for (size_t i = 0; i < inliers->indices.size(); i++) {
00494       pcl::PointNormal pp = cloud->points[inliers->indices[i]];
00495       const Eigen::Vector3f p = pp.getVector3fMap();
00496       const Eigen::Vector3f local_p = inv_pose * p;
00497       const int nx = floor(local_p[0] / dx);
00498       const int ny = floor(local_p[1] / dy);
00499       //std::cout << "abs_local_p2: " << std::abs(local_p[2]) << "/" << vertex_threshold << std::endl;
00500       if (0 <= nx && nx < foot_x_sampling_num &&
00501           0 <= ny && ny < foot_y_sampling_num &&
00502           std::abs(local_p[2]) < vertex_threshold) {
00503         occupiedp[nx][ny] = true;
00504       }
00505     }
00506     for (size_t i = 0; i < foot_x_sampling_num; i++) {
00507       for (size_t j = 0; j < foot_y_sampling_num; j++) {
00508         if (!occupiedp[i][j]) {
00509           return NOT_SUPPORTED;
00510         }
00511       }
00512     }
00513 
00514     Eigen::Vector3f a((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
00515     Eigen::Vector3f b((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
00516     Eigen::Vector3f c((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
00517     Eigen::Vector3f d((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
00518     pcl::PointNormal pa, pb, pc, pd, p;
00519     pa.getVector3fMap() = a;
00520     pb.getVector3fMap() = b;
00521     pc.getVector3fMap() = c;
00522     pd.getVector3fMap() = d;
00523     p.getVector3fMap() = Eigen::Vector3f(pose.translation());
00524     std::vector<int> kdl_indices;
00525     std::vector<float> kdl_distances;
00526     if (tree.radiusSearch(p, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
00527         tree.radiusSearch(pa, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
00528         tree.radiusSearch(pb, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
00529         tree.radiusSearch(pc, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
00530         tree.radiusSearch(pd, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00531       return SUPPORTED;
00532     }
00533     else {
00534       return CLOSE_TO_SUPPORTED;
00535     }
00536   }
00537   
00538   FootstepSupportState
00539   FootstepState::isSupportedByPointCloudWithoutCropping(
00540     const Eigen::Affine3f& pose,
00541     pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00542     pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00543     pcl::PointIndices::Ptr inliers,
00544     const int foot_x_sampling_num,
00545     const int foot_y_sampling_num,
00546     const double vertex_threshold)
00547   {
00548     const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
00549     const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
00550     Eigen::Vector3f a((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
00551     Eigen::Vector3f b((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
00552     Eigen::Vector3f c((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
00553     Eigen::Vector3f d((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
00554     pcl::PointNormal pa, pb, pc, pd, p;
00555     pa.getVector3fMap() = a;
00556     pb.getVector3fMap() = b;
00557     pc.getVector3fMap() = c;
00558     pd.getVector3fMap() = d;
00559     p.getVector3fMap() = Eigen::Vector3f(pose.translation());
00560     std::vector<int> kdl_indices;
00561     std::vector<float> kdl_distances;
00562     size_t success_count = 0;
00563     
00564     if (tree.radiusSearch(p, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00565       ++success_count;
00566     }
00567     if (tree.radiusSearch(pa, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00568       ++success_count;
00569     }
00570     if (tree.radiusSearch(pb, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00571       ++success_count;
00572     }
00573     if (tree.radiusSearch(pc, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00574       ++success_count;
00575     }
00576     if (tree.radiusSearch(pd, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
00577       ++success_count;
00578     }
00579     if (success_count == 5) {
00580       return SUPPORTED;
00581     }
00582     // else if (success_count > 0) {
00583     //   return CLOSE_TO_SUPPORTED;
00584     // }
00585     else {
00586       return NOT_SUPPORTED;
00587     }
00588   }
00589   
00590 
00591   
00592   FootstepState::Ptr FootstepState::fromROSMsg(const jsk_footstep_msgs::Footstep& f,
00593                                                const Eigen::Vector3f& size,
00594                                                const Eigen::Vector3f& resolution)
00595   {
00596     Eigen::Affine3f pose;
00597     Eigen::Vector3f offset (f.offset.x, f.offset.y, f.offset.z);
00598     tf::poseMsgToEigen(f.pose, pose);
00599     pose *= Eigen::Translation3f(offset);
00600     return FootstepState::Ptr(new FootstepState(
00601                                 f.leg, pose,
00602                                 size, resolution));
00603   }
00604 
00605   Eigen::Affine3f FootstepState::midcoords(const FootstepState& other)
00606   {
00607     Eigen::Affine3f first = pose_;
00608     Eigen::Affine3f second = other.pose_;
00609     Eigen::Translation3f pos((Eigen::Vector3f(first.translation()) + Eigen::Vector3f(second.translation())) / 2.0);
00610     Eigen::Quaternionf rot = Eigen::Quaternionf(first.matrix().block<3, 3>(0, 0)).slerp(0.5, Eigen::Quaternionf(second.matrix().block<3, 3>(0, 0)));
00611     return pos * rot;
00612     //return rot * pos;
00613   }
00614   
00615 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:29