先上一波效果圖：
整體實現的思路說穿了也很簡單，我總結如下：
需要注意的是每個座標系之間的關係，以我自己來說，我使用的是SICK-TIM561的二維激光雷達，這款雷達的座標系x軸向前，y軸向左，z軸向上。而雷達我這邊的安裝方式是距離地面0.5米，與水平面夾角45°安裝，因此，第一步就需要將激光雷達的座標系轉正，也就是繞着激光雷達的y軸逆時針旋轉45°。
第二步就是確定里程計的座標系，發現里程計座標系與旋轉後的激光雷達座標系xy平面平行，僅需要圍繞里程計z軸進行旋轉即可將二者座標系重合。此時即可發佈拼接後的點雲。
而獲取每個點雲的rgb值，則需要使用旋轉前的激光雷達座標系，將該座標系通過旋轉平移變換到相機的圖像座標系下，對應於每一組座標，若可以投影在圖像中，都可以取到對應的RGB值，而取值我使用的是cvGet2D函數。
最後，將點雲按照pcl::PointCloud<XYZRGB>進行發佈即可。