drl

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