姿態角（RPY）的優化目標函數

    在Pose-Graph的過程中，如果使用G2O優化函數庫，那麼似乎是不用自己編寫代價函數（也就是優化目標函數）的，因爲G2O有封裝好的SE3等格式，使得Pose-Graph的過程變得簡單了，即只需要設置好vertex（優化變量），以及edge（vertex與vertex之間的trans）,然後調用G2O的優化器，即可完成對Pose的優化；其實PoseGraph中的邊本質上就是一個代價函數，

>>阅读原文<<