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;
}