【ROS-資料格式】 csm 的資料格式

整理翻譯ROS中csm的資料格式，包括結構體sm_params和sm_result，翻譯和了解水準有限，有問題可以指出留言，我們一起進步!

• CSM的資料格式

CSM的資料格式

#ifndef H_SCAN_MATCHING_LIB
#define H_SCAN_MATCHING_LIB

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "hsm/hsm.h"
#include "hsm/hsm_interface.h"
#include "laser_data.h"


struct sm_params {

	/** First scan ("ref"erence scan) */
	//第一個雷達資料
	LDP laser_ref;
	
	/** Second scan ("sens"or scan) */
	//第二個雷達資料
	LDP laser_sens;

	/** Where to start */
	//位姿的預測值多少.設定為0 就是不預測
 	double first_guess[3]; 

	/** Maximum angular displacement between scans (deg)*/
	//掃描之間的最大角位移（度）
	double max_angular_correction_deg;
	
	/** Maximum translation between scans (m) */
	//掃描之間的最大平移（米）
	double max_linear_correction;

	/** When to stop */
	//最大疊代次數
	int max_iterations;
	
	/** A threshold for stopping. */
	//停止門檻值xy
	double epsilon_xy;
	
	/** A threshold for stopping. */
	//停止門檻值角度
	double epsilon_theta;
	
	/** Maximum distance for a correspondence to be valid */
	//通信有效的最大距離
	double max_correspondence_dist;
	
	/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
	//用一些聰明的技巧來尋找通信，隻影響速度;不收斂，這裡是直譯，暫時未能明白！
	int use_corr_tricks;
	
	/** Restart if error under threshold (0 or 1)*/
	//如果錯誤低于門檻值則重新啟動
	int restart;
	
		/** Threshold for restarting */
		//重新啟動的門檻值
		double restart_threshold_mean_error;
		
		/** Displacement for restarting */
		//重新啟動的位移
		double restart_dt;
		
		/** Displacement for restarting */
		//重新啟動的角度
		double restart_dtheta;
	

	/* Functions concerning discarding correspondences.
	   THESE ARE MAGIC NUMBERS -- and they need to be tuned. */
	//有關舍棄通信的功能-這些都是神奇的數字，需要調整的
	
	/** Percentage of correspondences to consider: if 0.9,
	    always discard the top 10% of correspondences with more error */
	//要考慮的通信比例：如果是0.9，則總是舍棄更大的前10%的通信
	double outliers_maxPerc;

	/** Parameters describing a simple adaptive algorithm for discarding.
	描述一個簡單的自适應舍棄算法的參數。
	
	    1) Order the errors.
		2) Choose the percentile according to outliers_adaptive_order.
		   (if it is 0.7, get the 70% percentile)
		3) Define an adaptive threshold multiplying outliers_adaptive_mult with the value of the error at the chosen percentile.
		4) Discard correspondences over the threshold.
		1)排序錯誤。
		2)根據outliers_adaptive_order選擇百分比。
(如果是0.7，選70%)
		3)将outliers_adaptive_mult與所選百分比的錯誤值相乘，定義一個自适應門檻值。
		4)丢棄超過門檻值的通信。
		
		This is useful to be conservative; yet remove the biggest errors.
		保守一點是有用的;移除最大的錯誤。
	*/
		double outliers_adaptive_order; /* 0.7 */
		double outliers_adaptive_mult; /* 2 */

	/** Do not allow two different correspondences to share a point */
	//不要讓兩個不同的通信者分享一個點
	//雷射傳感器中沒有兩個點具有相同的通信
	int outliers_remove_doubles; 


	
	/* Functions that compute and use point orientation for defining matches. */
	//計算和使用點方向來定義比對的函數。

		/** For now, a very simple max-distance clustering algorithm is used */
		//目前使用的是一種非常簡單的最大距離聚類算法
		double clustering_threshold;
		
		/** Number of neighbour rays used to estimate the orientation.*/
		//用來估計方向的鄰射線的數目。
		int orientation_neighbourhood;
		
		/** Discard correspondences based on the angles */
		//根據角度丢棄通信
		int do_alpha_test;
		double do_alpha_test_thresholdDeg;
		
		
	/** I believe this trick is documented in one of the papers by Guttman (but I can't find the reference). Or perhaps I was told by him directly. 
		我相信這個技巧在Guttman的一篇論文中有記載(但我找不到)
參考)。也許是他直接告訴我的。

		If you already have a guess of the solution, you can compute the polar angle of the points of one scan in the new position. If the polar angle is not a monotone function of the readings index, it means that the surface is not visible in the  next position. If it is not visible, then we don't use it for matching.
		如果你已經猜出了答案，你可以計算出在新位置的一個掃描點的極角。如果極角不是讀數指數的單調函數，則意味着表面在下一個位置不可見。如果它是不可見的，那麼我們就不使用它來比對。
		This is confusing without a picture! To understand what's going on, make a drawing in which a surface is not visible in one of the poses.
		沒有照片是很混亂的!為了了解發生了什麼，繪制一個在某個姿勢中不可見的表面。
	  	Implemented in the function visibilityTest().
	  	在函數visibilityTest()中實作。
	*/	
	int do_visibility_test;

	/** If 1, use PlICP; if 0, use vanilla ICP. */
	//如1，使用PlICP;如果為0，使用vanilla ICP。
	int use_point_to_line_distance;

	/** If 1, the field "true_alpha" is used to compute the incidence beta, and the factor (1/cos^2(beta)) used to weight the impact of each correspondence. This works fabolously if doing localization, that is the first scan has no noise.
	如果是1，則使用字段“true_alpha”來計算關聯beta，使用因子(1/cos^2(beta))來衡量每個對應的影響。這在進行定位時非常有效，因為第一次掃描沒有噪聲。
		If "true_alpha" is not available, it uses "alpha".
		如果“true_alpha”不可用，則使用“alpha”。
	*/
	int use_ml_weights;
	
	/* If 1, the field "readings_sigma" is used to weight the correspondence by 1/sigma^2 */
	//如果是1，“readings_sigma”字段用于對應的權重為1/sigma^2
	int use_sigma_weights;
	
	/** Use the method in http://purl.org/censi/2006/icpcov to compute the matching covariance. */
	//使用http://purl.org/censi/2006/icpcov中的方法計算比對協方差。
	int do_compute_covariance;

	/** Checks that find_correspondences_tricks give the right answer */
	//檢查find_correspondence es_tricks給出的答案是否正确
	int debug_verify_tricks;
	
	/** Pose of sensor with respect to robot: used for computing the first estimate given the odometry. */
	//傳感器相對于機器人的姿态:用于計算給定裡程計的第一個估計。
	double laser[3]; 

	/** Noise in the scan */
	//掃描噪聲
	double sigma;

	/** mark as invalid ( = don't use ) rays outside of this interval */
	//标記為無效(=不要使用)射線以外的這個間隔
	double min_reading, max_reading;
	
	/* Parameters specific to GPM (unfinished :-/ ) */
	//GPM參數(未完成:-/)
	double gpm_theta_bin_size_deg;
	double gpm_extend_range_deg; 
	int gpm_interval;
	/* Parameter specific to HSM (unfinished :-/ ) */
	//HSM專用參數(未完成:-/)
	struct hsm_params hsm;
};


struct sm_result {
	/** 1 if the result is valid */
	//如果結果有效
	int valid;
	
	/** Scan matching result (x,y,theta) */
	//掃描比對結果(x,y,theta)
	double x[3];
	
	/** Number of iterations done */
	//疊代次數
	int iterations;
	/** Number of valid correspondence in the end */
	//最終有效通信的數量
	int nvalid;
	/** Total correspondence error */
	//總通信錯誤
	double error;
	
	/** Fields used for covariance computation */
	//用于協方差計算的字段
	#ifndef RUBY
		gsl_matrix *cov_x_m;	
		gsl_matrix *dx_dy1_m;
		gsl_matrix *dx_dy2_m;
	#endif
};


void sm_icp(struct sm_params*input, struct sm_result*output);
void sm_icp_xy(struct sm_params*input, struct sm_result*output);
void sm_gpm(struct sm_params*input, struct sm_result*output);
void sm_hsm(struct sm_params*input, struct sm_result*output);

/* Unfinished, untested :-/ */
void sm_mbcip(struct sm_params*input, struct sm_result*output);



void sm_journal_open(const char* file);

#endif

};