ros的tf庫
為了這個題目，我是拿出了擠溝的精神擠時間，是下了功夫的，線性代數、矩陣論複習了，慣性導航裡的dcm、四元數也了解了，剛體力學也翻了，wiki裡的尤拉角也讀了，tf的tutorial、*****、source code也都看了。說實在的，經過這次努力，我是有點了然於胸了，我也非常想了然於紙上與小夥伴們分享，但嘗試了幾次失敗了，我也不跟自己過不去了，還是扯吧。
1）tf不是座標變換那麼簡單。
很多小夥伴認為tf的作用是便捷的進行座標變換。這個沒錯，但沒這麼簡單。
在很多api中，存在著target frame，source frame，parent frame，child frame，這些名字的引數。
看起來很讓人糊塗，也很讓讓人煩，但裡面隱藏著很多資訊。source、target frame是在進行座標變換時的概念，source是座標變換的源座標系，target是目標座標系，這個時候，這個變換代表的是座標變換。
parent、child frame是在描述座標系變換時的概念，parent是原座標系，child是變換後的座標系，這個時候這個變換描述的是座標系變換，也是child座標繫在parent座標系下的描述。如果把child固結於乙個剛體，那麼這個變換描述的就是剛體在parent座標系下的姿態。這個用處就大多了，可以用它來描述剛體的運動了。
這裡有個巧合，當然也不是巧合，那就是從child到parent的座標變換等於從parent到child的frame transform，等於child在parent frame的姿態描述。這裡牽扯到了線性代數裡的基變換、線性變換、過渡矩陣的概念。
2）簡單的說
不過，如果單說座標變換就簡單了。所謂座標變換就是把乙個點在某個座標系的描述，變換成在另外乙個座標系下的描述。
要實現這個變換，比較簡單的做法就是用當前座標繫在參考座標系下的描述（乙個矩陣）去描述（乘以）這個點在當前座標系下的描述（座標）。
所以只要我們能夠知道當前座標繫在參考座標系的描述，我們就可以把當前座標系裡任何乙個點的座標變換成參考座標系裡的座標。
好吧，不考慮平移，只考慮旋轉，參考座標系換個名字叫固定座標系，當前座標系叫動座標系，那麼該如何得到這個描述呢？
乙個基本的旋轉關係構成的矩陣是很好描述的。那麼多次旋轉呢？
3）多次旋轉、三次旋轉、尤拉角、四元數
經過多次旋轉的座標係該怎麼在固定座標系下描述呢。這裡有乙個前乘和後乘的問題。什麼時候前，什麼時候後，很簡單，繞固定座標系的軸旋轉就前乘，繞定座標系就後乘。為什麼是這樣呢？解釋起來很複雜，但可以這樣理解，繞固定軸旋轉時，你把這個旋轉看做是線性變換，這個變換把原來的矩陣裡的各列向量旋轉變換成了新的向量，所以前乘。當繞動座標系旋轉時，你把這個旋轉矩陣看做是乙個描述了，並且這個描述是在未乘之前的矩陣這個描述下的描述，所以要放到後面。
剛體的任一姿態（任一動座標系）可以經由三次基本旋轉得到，用三個角來描述，這就是尤拉角。在tf裡eulerypr指的是繞動座標系的zyx旋轉，rpy指的是繞固定座標系xyz旋轉，這二者等價，座標系定義為右手，x前，y左，z上。
四元數是剛體姿態的另一種描述方式，理論基礎是，剛體姿態可以經過某一特定軸經一次旋轉一定角度得到，等價於尤拉角，等價於旋轉矩陣。乙個四個引數，乙個三個引數，乙個九個引數，之所以等價是因為四個引數裡有乙個約束，九個引數裡有六個約束。
ps：這次就這樣了，下次再說tf的機制吧。
（8.2）ros的tf庫
4）tf的實現機制
4.1）有一棵樹
tf庫的目的是實現系統中任乙個點在所有座標系之間的座標變換，也就是說，只要給定乙個座標系下的乙個點的座標，就能獲得這個點在其他座標系的座標。
為了達到這個目的，就需要確定各個座標系之間的關係，也就是獲得任一座標係在其他任乙個座標系下的描述。
假設有n個座標系，那麼他們之間的組合關係有c（n，2）個，如果n=4，那就是6種，如果n=100，那就是4950個，好多呀。不過，世界沒那麼複雜，我們可以給這些座標系之間構建乙個結構，比如單叉樹或者單層多叉樹，那麼100個座標系之間的關係，就可以用99個樹杈搞定。tf裡的選擇則是多層多叉樹。
樹的結構特點是單親，乙個孩子只有父親，沒有母親，不允許兩個樹枝搞物件，長出果實，這也是tf裡說的沒有迴路的意思，為什麼這樣呢？因為怕矛盾，有孩子的都知道，對孩子的問題上，兩口子很難一致，不一致時，孩子會比較茫然，但每個小孩子都會趨利避害，它們往往會選擇比較厲害的一方，那就是母老虎，把不厲害的一方灰太狼遮蔽忽略掉。座標變換裡可沒有主體和選擇機制，所以還是傻傻的單親比較好處理。不過，有利必然有弊，弊端後面再說。
4.2）維護一顆樹
維護一顆樹，需要先設計乙個樹的結構，也就是一定要在樹被風吹來吹去變換的時候，讓小樹杈找個大樹杈依附，讓大樹杈找個樹幹依附，讓樹幹有個根固定。（這裡提一下我的建議，免得一會忘了，建議是，如果打算用tf解決你的座標變換問題，請一定要先清晰的畫出這棵樹的結構，再開始寫程式；因為這個結構是維護和使用的基礎）。
這個結構的建立和維護靠的是tf提供的tfbroadcastor類的sendtransform介面。在tb第一次發布乙個從已有的parent frame到新的child frame的座標系變換時，這棵樹就會新增乙個樹枝，之後就是維護。
這裡，提醒一下，在用tf時，一定要細心照料整棵大樹，時刻保持這棵大樹能夠描述整個外部世界裡的關係，不能有斷裂。這樣才能保證在風吹時，整個樹都擺來擺去還保持穩定。
4.3）使用這棵樹
很簡單，用tf的tflisener監聽某乙個指定的從乙個a frame到b frame的變換即可，當然前提是樹上的樹杈們能把a，b聯通，並已經準備好。再重複一次，這個變換是a frame到b frame的變換，也表示了b frame在a frame的描述，也代表了把乙個點在b frame裡的座標變換成在a frame裡的座標的座標變換。有了這個變換，你就可以盡情的變換了。
4.4）實現的原理
實現的原理，非常簡單，也非常粗暴。
基本原理是，tb的類裡有個publisher，tl的類裡有個subscriber，乙個發布叫/tf的topic，乙個訂閱這個topic，傳送的訊息tfmessage裡包含了parent frameid和child frameid的資訊。
這個機制意味著，所有的tb會發布某一特定的parent到child的變換，而所有tl會收到所有的這些變換，然後tl利用乙個tfbuffercore的資料結構維護乙個完整的樹結構及其狀態。基於此，tl在使用這棵樹時，會用lookuptransform或waitfortransform來獲得任意座標系之間的變換。
非常簡單，對吧，但粗暴的也非常顯然。那就是只要是乙個tl，就要跟所有tb建立連線，就要收取所有的/tf訊息，來維護一棵完整的樹，並且，還要負責搜尋這棵樹，找到一條變換的路徑，然後乘呀乘，並且每次都要如此。太粗暴了！粗暴到一定程就是愚蠢了，可是為了活得簡單，我們往往會玩粗暴。
下一節（8.3）裡再扯第5）節tf庫的優缺點。
還是寫完再補覺吧，晚上還有其他作業。
（8.3）ros裡的tf庫
5）tf庫的優缺點
先說優點，能用且易上手。
a、各種數值計算的細節，你不用考慮，tf庫可以幫你；
b、介面很簡潔，會廣播和監聽就ok；
c、問題找的很準，那就是需要維護座標系之間的關係；
d、提供了很多任務具程式；
e、考慮了與時間相關的變換；
f、支援tf-prefix，可以在多機械人上用
g、基本能用，且不需要深入理解，死搬硬套就能用起來。
當然還有很多優點，我就不說了，不太擅長，還是說缺點吧。
還是那句話，簡單粗暴！
a、樹的結構很簡單，但有時候很笨拙。舉個例子吧。有部電影叫手機，葛優扮演的叫石頭，范偉演的叫磚頭，兩個人是叔伯兄弟，從小到大的小夥伴，應該很熟悉，關係也很明確，但要放到樹的結構下，就需要從下到上找共同先輩，然後從這個先輩再往下找，進而確定二者的關係，這個比較笨拙。於是有了范偉坐在門檻上的一句台詞：恁這是弄啥勒，恁奶不是俺奶？！
b、每個tl都要維護同一顆樹，這樣的開銷太大，主要是網路傳輸的負荷比較大。這一點，tf的設計者是完全承認的。舉個例子吧，北方的有些地方的農村過年時要拜年，拜年要磕頭，磕頭的物件是寫有逝去的先輩名字的樹狀圖，乙個同姓大家族裡有很多小家庭，每個家裡都有那麼一張圖，挨家拜年，街上人來人往，每個人都磕了很多頭，磕的其實是同一張。如果有個祠堂，集中處理，維護一張圖，就不用看到早早起床、滿街跑的情況，效率也會高多了。
c、很難滿足實時性的要求，這一點比較顯然。要不tf也不會每個變換存10秒鐘的資料，不過原始碼裡好像是開了乙個存100個訊息的佇列。
d、還有很多細節不易理解。比如，now（）和time（0）；比如，技術文件裡的一些術語名詞；比如，採用了機械人裡的習慣，與飛行器，慣導，車輛裡的習慣區別較大，使用時不能相當然。
綜上，用不用tf需要平衡，那裡用，那裡不用，用什麼代替，也需要平衡，不是一件很輕鬆愉快的事。
6）扯點其他的
有個我最喜歡的系列電影，叫黑客帝國，英文名字叫matrix，也就是矩陣。之所以喜歡是因為從一開始到現在我都沒看懂。
乙個矩陣實際上定義了乙個描述的視角。對空間裡的乙個點做描述，乙個點比較絕對的描述是mx，其中m是座標系的姿態，x是座標向量，mx的含義就是在座標系m裡座標為x的那個點。對於y=mx，你可以看作是iy=mx。也就是說，在i座標系裡座標為y的乙個點和在m系裡座標為x的是同乙個點。
所以，虛擬世界裡的工程師安德森與錫安裡的尼歐是同乙個點在不同座標系下不同的座標描述。
問題的核心是這兩個座標系沒有乙個是i系，所以儘管你清楚，n安德森=m尼歐，但在n的世界裡卻不能確定n是什麼，僅知道m是什麼，在m的世界則相反。所以在錫安鏈結虛擬世界時，實際上是用n的逆乘以m尼歐得到了安德森。
問題來了，當the one i 出現的時候，變成了ntheone=m尼歐，既ni=m尼歐，也就是n=m尼歐，所以m世界裡就知道了m的絕對意義，就會認為m是真實世界。也獲取了通過改變尼歐的座標屬性，然後使得n逆乘以m再乘以尼歐變成n世界裡最牛的人。不過，這個m只有尼歐和the one在變換過程中才能知道。
隨著尼歐在錫安這個真實世界裡也把自己當作了the one，奇蹟發生了。那就是ni=mi，既n=m，錫安和虛擬世界沒有區別，都在同乙個座標系下，這樣尼歐在錫安世界裡也具備了在虛擬世界裡的超能力，以the one的身份展開了與機械人的對話，告訴機械人別太認真了，大家都tm的是程式。
以上純屬閒扯，不許反駁，不是我怕反駁，而是我更喜歡聽到您的閒扯。
ROS中觀察TF的工具
1 使用 view frames 建立乙個在ros上，由tf發布的座標系的。2 使用rqt tf tree 這是乙個實時工具，觀察在ros上被發布的座標系樹，可用重新整理按鈕來更新樹的內容。與上乙個工具的區別在於 上乙個工具連續取樣5s獲得的樹的內容，並存成乙個 這個工具可以連續的觀察樹的內容，使用...
Ros中tf的相關功能包
能夠監聽當前時刻所有通過ros廣播的tf座標系，並繪製出樹狀圖表示座標系之間的連線關係儲存到離線檔案中 rosrun tf view frames雖然view frames能夠將當前座標系關係儲存在離線檔案中，但是無法實時反映座標關係，所以可以用rqt tf tree實時重新整理顯示座標系關係 ro...
TF庫的使用
1 tf不是座標變換那麼簡單。很多小夥伴認為tf的作用是便捷的進行座標變換。這個沒錯，但沒這麼簡單。在很多api中，存在著 target frame，source frame，parent frame，child frame 這些名字的引數。看起來很讓人糊塗，也很讓讓人煩，但裡面隱藏著很多資訊。so...