FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in, unsigned int& error_state) { if(!use_pointcloud_model_) { return in; } ros::WallTime start_time = ros::WallTime::now(); FootstepState::Ptr projected_footstep = in->projectToCloud( *tree_model_, pointcloud_model_, grid_search_, *tree_model_2d_, pointcloud_model_2d_, Eigen::Vector3f(0, 0, 1), error_state, parameters_); ros::WallTime end_time = ros::WallTime::now(); perception_duration_ = perception_duration_ + (end_time - start_time); return projected_footstep; }
FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in, unsigned int& error_state) { ros::WallTime start_time = ros::WallTime::now(); FootstepState::Ptr projected_footstep = in->projectToCloud( *tree_model_, pointcloud_model_, grid_search_, *tree_model_2d_, pointcloud_model_2d_, Eigen::Vector3f(0, 0, 1), error_state, plane_estimation_outlier_threshold_, plane_estimation_max_iterations_, plane_estimation_min_inliers_, support_check_x_sampling_, support_check_y_sampling_, support_check_vertex_neighbor_threshold_); ros::WallTime end_time = ros::WallTime::now(); perception_duration_ = perception_duration_ + (end_time - start_time); return projected_footstep; }