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>())
// pose_tf2_transform_corrected_(修正结果) pose_corrections(修正)
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(initial_pose_estimators_feature_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) {
last_scan_time_ = pose_time;
if (publish_tf_map_odom_) {
pose_to_tf_publisher_->publishTF(pose_tf_corrected_to_publish, ambient_cloud_time, ambient_cloud_time);
}
last_accepted_pose_odom_to_map_ = pose_tf2_transform_corrected_ * transform_base_link_to_odom.inverse();
tf2::Transform pose_tf_corrected_to_publish = pose_tf2_transform_corrected_;
laserscan_to_pointcloud::tf_rosmsg_eigen_conversions::transformTF2ToMsg(pose_tf_corrected_to_publish, pose_corrected_msg->pose);
pose_stamped_publisher_.publish(pose_corrected_msg);
}else
{
++pose_tracking_number_of_failed_registrations_since_last_valid_pose_;
}
received_external_initial_pose_estimation_ = false;
}
}
}
template<typename PointT>
void Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const std::string& frame_id, const ros::Time& pose_time) {
tf2::Transform transform_base_link_to_map(pose ...); // get base_link to map from pose
pose_to_tf_publisher_->getTfCollector().lookForTransform(transform_odom_to_base_link, base_link_frame_id_, odom_frame_id_, pose_time, tf_timeout_)) {
tf2::Transform last_accepted_pose_odom_to_map = transform_base_link_to_map * transform_odom_to_base_link;
last_accepted_pose_base_link_to_map_ = transform_base_link_to_map;
last_accepted_pose_odom_to_map_ = last_accepted_pose_odom_to_map;
last_accepted_pose_time_ = pose_time_updated;
last_accepted_pose_valid_ = true;
pose_tracking_number_of_failed_registrations_since_last_valid_pose_ = 0;
received_external_initial_pose_estimation_ = true;
}
template<typename PointT>
void Localization<PointT>::loadReferencePointCloudFromROSOccupancyGrid(const nav_msgs::OccupancyGridConstPtr& occupancy_grid_msg) {
if (ros::Time::now() - last_map_received_time_ > min_seconds_between_reference_pointcloud_update_) {
typename pcl::PointCloud<PointT>::Ptr reference_pointcloud_from_occupancy_grid(new pcl::PointCloud<PointT>());
pointcloud_conversions::fromROSMsg(*occupancy_grid_msg, *reference_pointcloud_from_occupancy_grid);
reference_pointcloud_2d_ = true;
reference_pointcloud_ = reference_pointcloud_from_occupancy_grid;
if (flip_normals_using_occupancy_grid_analysis_ && reference_cloud_normal_estimator_) reference_cloud_normal_estimator_->setOccupancyGridMsg(occupancy_grid_msg);
updateLocalizationPipelineWithNewReferenceCloud(occupancy_grid_msg->header.stamp)
{
reference_pointcloud_->header.frame_id = map_frame_id_for_transforming_pointclouds_;
reference_pointcloud_->header.stamp = pcl_conversions::toPCL(time_stamp);
typename pcl::PointCloud<PointT>::Ptr reference_pointcloud_raw;
if (!use_filtered_cloud_as_normal_estimation_surface_reference_) { //false
reference_pointcloud_raw = typename pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>(*reference_pointcloud_));
}
applyFilters(reference_cloud_filters_, reference_pointcloud_);
reference_pointcloud_search_method_->setInputCloud(reference_pointcloud_);
applyNormalEstimation(reference_cloud_normal_estimator_, reference_cloud_curvature_estimator_, reference_pointcloud_, reference_pointcloud_raw, reference_pointcloud_search_method_, true)) { return false; }
for (size_t i = 0; i < reference_pointcloud_->size(); ++i) {
(*reference_pointcloud_)[i].getNormalVector3fMap().normalize();
applyKeypointDetection(reference_cloud_keypoint_detectors_, reference_pointcloud_, reference_pointcloud_search_method_, reference_pointcloud_keypoints_);
updateMatchersReferenceCloud()
{
for (size_t i = 0; i < initial_pose_estimators_feature_matchers_.size(); ++i) {
initial_pose_estimators_feature_matchers_[i]->setupReferenceCloud(reference_pointcloud_, reference_pointcloud_keypoints_, reference_pointcloud_search_method_);
}
for (size_t i = 0; i < initial_pose_estimators_point_matchers_.size(); ++i) {
initial_pose_estimators_point_matchers_[i]->setupReferenceCloud(reference_pointcloud_, reference_pointcloud_keypoints_, reference_pointcloud_search_method_);
}
for (size_t i = 0; i < tracking_matchers_.size(); ++i) {
tracking_matchers_[i]->setupReferenceCloud(reference_pointcloud_, reference_pointcloud_keypoints_, reference_pointcloud_search_method_);
}
for (size_t i = 0; i < tracking_recovery_matchers_.size(); ++i) {
tracking_recovery_matchers_[i]->setupReferenceCloud(reference_pointcloud_, reference_pointcloud_keypoints_, reference_pointcloud_search_method_);
}
}
publishReferencePointCloud(time_stamp, true);
reference_pointcloud_loaded_ = true;
return true;
}
}
}