drl
template<typename PointT>
void Localization<PointT>::processAmbientPointCloud(const sensor_msgs::PointCloud2ConstPtr& ambient_cloud_msg) {
// 检查是否需要处理数据，
if (checkIfAmbientPointCloudShouldBeProcessed(ambient_cloud_time, number_points_ambient_pointcloud, true, true))
{
processAmbientPointCloud(ambient_pointcloud, false, false)
{
checkIfAmbientPointCloudShouldBeProcessed(ambient_cloud_time, number_points_ambient_pointcloud, check_if_pointcloud_subscribers_are_active, true)
if(checkIfTrackingIsLost()) if(reset_initial_pose_when_tracking_is_lost_(false)) (setupInitialPose());
pose_to_tf_publisher_->getTfCollector().lookForTransform(transform_base_link_to_odom, ...) //订阅里程计
pose_tf_initial_guess = last_accepted_pose_odom_to_map_ * transform_base_link_to_odom;
typename pcl::PointCloud<PointT>::Ptr ambient_pointcloud_keypoints(new pcl::PointCloud<PointT>())
bool localizationUpdateSuccess = updateLocalizationWithAmbientPointCloud(ambient_pointcloud, ambient_cloud_time, pose_tf_initial_guess, pose_tf2_transform_corrected_, pose_corrections, ambient_pointcloud_keypoints);
{
bool lost_tracking = checkIfTrackingIsLost();
transformCloudToTFFrame(ambient_pointcloud_raw, pointcloud_time, "map")
{
pose_to_tf_publisher_->getTfCollector().lookForTransform(pose_tf_cloud_to_odom...) // laser 到 odom（里程计pose+laser到base—link偏差）
pose_tf_cloud_to_map = last_accepted_pose_odom_to_map_ * pose_tf_cloud_to_odom; //laser - map
pcl::transformPointCloudWithNormals(*ambient_pointcloud, *ambient_pointcloud, pose_tf_cloud_to_map_eigen_transform);
}
resetPointCloudHeight(*ambient_pointcloud_raw); //设置z为固定值、
//******************************************************filters ambient_pointcloud_feature_registration_filters_ is null
applyFilters(lost_tracking ? ambient_pointcloud_feature_registration_filters_ : ambient_pointcloud_filters_, ambient_pointcloud)
if (ambient_pointcloud->size() < ...) return false;
//******************************************************normal estimation
bool computed_normals = false;
compute_normals_when_tracking_pose_ (false): do nothing;
//******************************************************keypoint selection
bool computed_keypoints = false;
compute_keypoints_when_tracking_pose_(false): do nothing;
// ============================================================== initial pose estimation when tracking is lost
bool tracking_recovery_reached = time_from_last_pose > pose_tracking_recovery_timeout_ &&pose_tracking_recovery_minimum_number_of_failed_registrations_since_last_valid_pose_ 3
|| > pose_tracking_recovery_maximum_number_of_failed_registrations_since_last_valid_pose_ 5
bool performed_recovery = false;
if(lost_tracking || received_external_initial_pose_estimation_){ // lost or get external_initial_pose_estimation
if (!received_external_initial_pose_estimation_){ // which mearns we lost
if (time_from_last_pose < initial_pose_estimation_timeout_(600.0)) {
if(!computed_normals){
applyNormalEstimation(ambient_cloud_normal_estimator_, ambient_cloud_curvature_estimator_, ambient_pointcloud, ambient_pointcloud_raw, ambient_search_method)
computed_normals = true;
}
if(!computed_keypoints){
applyKeypointDetection(ambient_cloud_keypoint_detectors_, ambient_pointcloud, ambient_search_method, ambient_pointcloud_keypoints_out);
computed_keypoints = true;
}
applyCloudRegistration(tracking_recovery_matchers_, ambient_pointcloud, ambient_search_method, (ambient_pointcloud_keypoints_out->size() < (size_t)minimum_number_of_points_in_ambient_pointcloud_(10)) ? ambient_pointcloud : ambient_pointcloud_keypoints_out, pose_corrections_out);
}else{
ROS_INFO(time_from_last_pose);
}
}
applyCloudRegistration(initial_pose_estimators_point_matchers_, ambient_pointcloud, ambient_search_method, (ambient_pointcloud_keypoints_out->size() < (size_t)minimum_number_of_points_in_ambient_pointcloud_) ? ambient_pointcloud : ambient_pointcloud_keypoints_out, pose_corrections_out);
ROS_DEBUG("Successfully performed initial pose estimation");
}else // ============================================================== point cloud registration with recovery
{
if(!applyCloudRegistration(tracking_matchers_, ambient_pointcloud, ambient_search_method, (ambient_pointcloud_keypoints_out->size() < (size_t)minimum_number_of_points_in_ambient_pointcloud_) ? ambient_pointcloud : ambient_pointcloud_keypoints_out, pose_corrections_out))
{
if(tracking_recovery_reached)
{
applyCloudRegistration(tracking_recovery_matchers_, ambient_pointcloud, ambient_search_method, (ambient_pointcloud_keypoints_out->size() < (size_t)minimum_number_of_points_in_ambient_pointcloud_) ? ambient_pointcloud : ambient_pointcloud_keypoints_out, pose_corrections_out);
performed_recovery = true;
}
}
}
// 把点云和keypoint通过以上位置修正结果纠正一下。
pointcloud_pose_corrected_out = pose_corrections_out * pointcloud_pose_initial_guess;
tf2::Transform post_process_cloud_registration_pose_corrections; // 对pose_corrections_out 再一次纠正
postProcessCloudRegistration(pointcloud_pose_initial_guess, pointcloud_pose_corrected_out, post_process_cloud_registration_pose_corrections, pointcloud_time);
pcl::transformPointCloudWithNormals(*ambient_pointcloud, *ambient_pointcloud, laserscan_to_pointcloud::tf_rosmsg_eigen_conversions::transformTF2ToTransform<double>(post_process_cloud_registration_pose_corrections));
pcl::transformPointCloudWithNormals(*ambient_pointcloud_keypoints_out, *ambient_pointcloud_keypoints_out, laserscan_to_pointcloud::tf_rosmsg_eigen_conversions::transformTF2ToTransform<double>(post_process_cloud_registration_pose_corrections));
pose_corrections_out = post_process_cloud_registration_pose_corrections * pose_corrections_out;
pointcloud_pose_corrected_out = pose_corrections_out * pointcloud_pose_initial_guess;
// ============================================================== outlier&inlierdetection
applyAmbientPointCloudOutlierDetection(ambient_pointcloud_integration ? ambient_pointcloud_integration : ambient_pointcloud);
applyReferencePointCloudOutlierDetection(ambient_search_method, reference_pointcloud_);
// ============================================================== localization post processors with registration recovery
applyCloudAnalysis(pointcloud_pose_corrected_out); // 分析 outliner 和 inliner 的角度值 ，得到outliers_angular_distribution_ 和 inliers_angular_distribution_
if(performed_recovery){
applyTransformationValidators(transformation_validators_tracking_recovery_, pointcloud_pose_initial_guess, pointcloud_pose_corrected_out, outlier_percentage_, outlier_percentage_reference_pointcloud_)
}else
{
if (!applyTransformationValidators(lost_tracking ? transformation_validators_initial_alignment_ : transformation_validators_, pointcloud_pose_initial_guess, pointcloud_pose_corrected_out, outlier_percentage_, outlier_percentage_reference_pointcloud_)){
if (!performed_recovery)
{
applyCloudRegistration(tracking_recovery_matchers_, ambient_pointcloud, ambient_search_method, (ambient_pointcloud_keypoints_out->size() < (size_t)minimum_number_of_points_in_ambient_pointcloud_) ? ambient_pointcloud : ambient_pointcloud_keypoints_out, pose_corrections_out);
// 同上 ，把点云和keypoint通过以上位置修正结果纠正一下;outlier&inlierdetection;分析 outliner 和 inliner 的角度值
applyTransformationValidators(transformation_validators_tracking_recovery_, pointcloud_pose_initial_guess, pointcloud_pose_corrected_out, outlier_percentage_, outlier_percentage_reference_pointcloud_)
}
}
}
updateMatchersReferenceCloud();
publishReferencePointCloud(time_stamp, true);
reference_pointcloud_loaded_ = true;
return true;
}
if (localizationUpdateSuccess) {
}else
{
++pose_tracking_number_of_failed_registrations_since_last_valid_pose_;
}
received_external_initial_pose_estimation_ = false;
}
}
}
processAmbientPointCloud(ambient_pointcloud, false, false)
limit_of_pointclouds_to_process : 只处理几帧数据，默认-1，处理所有数据
checkIfAmbientPointCloudShouldBeProcessed(ambient_cloud_time, number_points_ambient_pointcloud, true, true) 下面的现象很难发生
ambient_pointcloud_subscribers_active_: ("Discarded point cloud because the subscribers are not active"); 没又订阅激光
reference_pointcloud_loaded_: ("Discarded cloud because there is no reference cloud to compare to"); 没有参考地图
minimum_number_of_points_in_ambient_pointcloud_(10) ： "Discarded ambient cloud [ minimum number of points required:10"
too_old : "Discarded ambient cloud because it's timestamp older than an already processed ambient cloud " 数据是旧的
min_seconds_between_scan_registration_(0.0): "Discarded cloud with an elapsed_time_since_last_scan is lower than the minimum allowed" 频率太高
max_seconds_ambient_pointcloud_age_(3.0): "Discarded cloud with scan_age higher than the maximum allowed " 数据太老
checkIfTrackingIsLost()
pose_tracking_timeout_(30.0)
&& pose_tracking_minimum_number_of_failed_registrations_since_last_valid_pose_ (25) : 失败次数太多 并且 当前时间与上次定位时间间隔太长
|| pose_tracking_maximum_number_of_failed_registrations_since_last_valid_pose_（50）; 失败次数特别特别多
updateLocalizationWithAmbientPointCloud(typename pcl::PointCloud<PointT>::Ptr& ambient_pointcloud,
const ros::Time& pointcloud_time,
const tf2::Transform& pointcloud_pose_initial_guess,
tf2::Transform& pointcloud_pose_corrected_out,
tf2::Transform& pose_corrections_out,
typename pcl::PointCloud<PointT>::Ptr ambient_pointcloud_keypoints_out)
