---
title: 'SDC midterm project: Localization'
disqus: hackmd
---

自駕車期中作業
===

## 程式架構圖

程式解說
---
### 1. 讀取地圖

//load map
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/stonelab510/catkin_ws/src/sdc_mid_1/itri_map.pcd", *cloud_map) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file map.pcd \n");
return (-1);
}
std::cout << "cloud Loaded "<< std::endl;

//down sampling
pcl::VoxelGrid<pcl::PointXYZ> sor2;
sor2.setInputCloud (cloud_map);
sor2.setLeafSize (1.0f, 1.0f, 1.0f);
sor2.filter (*cloud_map_filtered);

在打開ros之前先讀取itri.pcd，讀取成功後對地圖做縮減採樣，除了減少計算量也可以排出雜訊。
### 2. 初始化

//initialization
float degree = 140.3 ;
Eigen::Matrix4f initial_guess;
initial_guess <<  cos ( degree * PI / 180.0 ),-sin ( degree * PI / 180.0 ),0.0, -284.884,
                  sin ( degree * PI / 180.0 ), cos ( degree * PI / 180.0 ),0.0,  226.111,
                                          0.0,                         0.0,1.0, -12.5952,
                                          0.0,                         0.0,0.0,     1.0;
transformation = initial_guess;
eigen_C_now = initial_guess.topLeftCorner<3,3>();

使用GPS的第一個值作為icp的初始值，並使用`yaw = 140.3(deg)`(以第一題為例)作為初始姿態。
`eigen_C_now`則儲存了第一筆3x3的旋轉矩陣，作為後續姿態更新的初始值。

ros::init (argc, argv, "pcl_write");
ros::NodeHandle nh;

//訂閱topic: /lidar_points
ros::Subscriber sub_lidar = nh.subscribe("/lidar_points",10000,fixed_frame_cb); 
ros::Subscriber sub_imu = nh.subscribe("/imu/data",1000,callback_imu);
pub2 = nh.advertise<sensor_msgs::PointCloud2>("filterd_lidar",1000);
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);

初始化第二部分則是對ros做初始化:
* 設定Subscriber分別訂閱`/lidar_points`, `/imu/data`，
當收到topic時會進到兩者的callback`fixed_frame_cb`和`callback_imu`。
* 設定Publisher`/filtered_lidar`負責發佈座標轉換後的雲點圖, `/odom`發佈Icp算出的位置及姿態。

### 3. Callback function
### /imu/data:

void callback_imu(const sensor_msgs::Imu::ConstPtr& imu_msg)
{ 
  imu_t_now = imu_msg->header.stamp.toSec();
  float dt = 0.1;
  float wx = imu_msg->angular_velocity.x;
  float wy = imu_msg->angular_velocity.y;
  float wz = imu_msg->angular_velocity.z;
  float sigma = sqrt(wx*wx + wy*wy + wz*wz)*dt;
  Eigen::Matrix3f B;
  B << 0, -wz*dt, wy*dt, wz*dt, 0, -wx*dt, -wy*dt, wx*dt, 0;
  
  tf::Matrix3x3 C_now;
  Eigen::Vector3f ea;
  Eigen::Matrix3f eigen_C_next;

  double roll, pitch, yaw;
  eigen_C_next = eigen_C_now*(Eigen::Matrix3f::Identity(3,3) + sin(sigma)/sigma*B + (1-cos(sigma))/sigma/sigma*B*B);
  imu_t_old = imu_t_now;
  transformation.topLeftCorner<3,3>() = eigen_C_next;
  ea = eigen_C_next.eulerAngles(2,1,0); 
}

5~8行從imu獲得body frame的角速度`wx`, `wy`, `wz`, 並計算$\sigma$ 以及`B`。

$\sigma = \left| {\omega dt} \right|$,

$B = \left( {\begin{array}{*{20}{c}}
{\rm{0}}&{ - {\omega _z}}&{{\omega _y}}\\
{{\omega _z}}&{\rm{0}}&{ - {\omega _x}}\\
{ - {\omega _y}}&{{\omega _x}}&{\rm{0}}
\end{array}} \right)$

17~19行是更新下一次做icp的初始值, 根據下式做更新。

$C(t + dt) = C(t)\left( {I + \frac{{\sin (\sigma )}}{\sigma }B + \frac{{1 - \cos (\sigma )}}{{{\sigma ^2}}}{B^2}} \right)$

### /lidar_points:

void fixed_frame_cb(const sensor_msgs::PointCloud2ConstPtr& pc2_msg)
{ 
  ...
//做downsampling
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud_in);
  sor.setLeafSize (1.0f, 1.0f, 1.0f);
  sor.filter (*cloud_filtered);
  
//Transform to baselink
  Eigen::Matrix4f eigen_transform;
  eigen_transform <<  0.998607,  0.0382136,  0.0363887,       0.46,
                    -0.0385865,   0.999209, 0.00960189,          0,
                     -0.035993, -0.0109926,   0.999292,       3.46,
                             0,        0.0,        0.0,          1;
  pcl::transformPointCloud (*cloud_filtered,*cloud_transformed, eigen_transform);

對`/lidar_points`降低採樣, `eigen_transform` 存放的是`/velodyne`到`/baselink`的轉換，因為是固定的值，所以直接寫成定值，減少運算時間。

//做icp
pcl::PointCloud<pcl::PointXYZ> Final;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_transformed);
icp.setInputTarget(cloud_map_filtered);
icp.setMaxCorrespondenceDistance (1.0);
icp.setMaximumIterations (1000);
icp.setTransformationEpsilon (1e-8);
icp.setEuclideanFitnessEpsilon (1e-8);

//轉到world
icp.align(Final, transformation);
transformation = icp.getFinalTransformation ();
pcl::transformPointCloud (*cloud_transformed, *cloud_icp_result, transformation);
std::cout << "icp done. "<< std::endl;
std::cout << icp.getFitnessScore() << std::endl;

//Publish odometry msg to /world
Eigen::Transform<float, 3, Eigen::Affine> tROTA(transformation);
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(tROTA, x, y, z, roll, pitch, yaw);
eigen_C_now = transformation.topLeftCorner<3,3>();

//publish icp result
sensor_msgs::PointCloud2 output;
pcl::toROSMsg( *cloud_icp_result, output);
output.header.frame_id = "world";
pub2.publish(output);

2~9行: 設定源點雲和目標點雲、icp的參數以及收斂標準，分別為對應點對之間的最大距離、最大迭代次數、兩次變化矩陣之間的差值、收斂條件是均方誤差和閾值。

12~13行: 是icp做完的結果為`world`到`base_link`的變換矩陣，含有3*3的旋轉矩陣以及位置資訊。

14行是將`base_link`的`/lidar_point`轉換到`world`。
`icp.getFitnessScore()`可以知道每一次迭代的分數，越小代表兩個雲點圖的差異越小。

21行使用` pcl::getTranslationAndEulerAngles`來獲取變換矩陣轉換成旋轉矩陣、位置、姿態資訊。

22行將最新的姿態估測值存到`eigen_C_now`全域變數，在imu的callback中可以用來估算下一次做icp的初始姿態。

25~28行將算出的結果發布`/filterd_lidar`到`world`frame。
#### Publish odom msg

tf::Quaternion q3;
q3.setRPY(roll, pitch, yaw);
q3.normalize();
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(q3, odom_quat);

nav_msgs::Odometry odom;
// odom.header.stamp = current_time;
odom.header.frame_id = "world";
odom.child_frame_id = "base_link";

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = z;
odom.pose.pose.orientation = odom_quat;

//set the velocity
float dt = 0.1;
// odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = (x - x_old)/dt;
odom.twist.twist.linear.y = (y - y_old)/dt;
odom.twist.twist.angular.z = (z - z_old)/dt;

odom_pub.publish(odom);

從icp算出的變換矩陣再轉換成四元數，並設為`odom`的姿態，在取變換矩陣的最後一欄設為`odom`的位置。

速度項是發佈在`base_link`上，因為速度的描述會在物體的坐標系上。其值為上個位置與此刻算出的結果的差值除以時間差。
#### Update

//update
x_old = x;
y_old = y;
z_old = z;

// update x, y position with dynamics
transformation(0,3) = x + dt*(odom.twist.twist.linear.x + dt*imu.linear_acceleration.x);
transformation(1,3) = y + dt*(odom.twist.twist.linear.y + dt*imu.linear_acceleration.y);
transformation(2,3) = z;

//write csv file
plottingFile.open("odomPlot.csv",std::ios::app);
plottingFile << cnt << "," << x << "," << y << "," << z << "," << yaw << "," << pitch << ","<< roll << std::endl;
plottingFile.close(); 
cnt = cnt + 1;

最後一步是更新下一個icp的初始值。
將新的x, y, z以動態方程式預測出下個時刻的估測值，但因為z軸方向的變化很小，因此不做估測。

12~14行將結果寫入`odomPlot.csv`中。

#### Select map

std::string* map_select(float x, float y){
  //找中間那塊地圖
  int x_ = int(x) - int(x)%100;
  int y_ = int(y) - int(y)%100;
  //找九宮格的地圖
  int map_x[3] = {x_ - 100, x_, x_ + 100};
  int map_y[3] = {y_ - 100, y_, y_ + 100};

  //存取地圖名稱
  std::stringstream map;
  static std::string map_name[9];
  int k = 0;
  for (int i=0;i<3;i++){
    for(int j=0;j<3;j++){
      map << "map_" << map_x[i] << "_" << map_y[j] <<".pcd";
      map_name[k] = map.str();
      k++;
      map.str("");
      map.clear();
    }
  }
  return map_name;
}

第一筆GPS為上圖綠點，則找出紅點座標，再把周圍八個地圖的座標找出。
將地圖名稱儲存在`map_name`中。

#### Load map

file_name = "/home/stonelab510/catkin_ws/src/sdc_mid_2/nuscenes_maps/" + map_name[i];

if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_name, *cloud_unit) == -1) //* load the file
{
 PCL_ERROR ("Couldn't read file map.pcd \n");
}
std::cout << i+1 << " ";
std::cout << file_name << std::endl;
*cloud_combined = *cloud_unit + *cloud_combined;
}
std::cout << "cloud Loaded "<< std::endl;

pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud_combined);
sor.setLeafSize (0.5f, 0.5f, 0.5f);
sor.filter (*cloud_filtered_seg);
std::cout << "cloud filterd. "<< std::endl;

pcl::toROSMsg(*cloud_filtered_seg, output_map);
return cloud_filtered_seg;

第9行是依序將地圖合併後再downsampling。

Problems and Solutions
---
>問題一:
>一開始的做法是**每一步都用gps當初始值**，上圖為第三題的結果截圖。可以看到所得到的結果會因為gps本身的跳動而導致結果不連續。也有觀察到在轉彎的過程中`icpscore`數值大幅提升(平移約0.3、轉彎約90)，推論是初始姿態所導致。

解決:參考[此文章](chrome-extension://efaidnbmnnnibpcajpcglclefindmkaj/viewer.html?pdfurl=https%3A%2F%2Fwww.cl.cam.ac.uk%2Ftechreports%2FUCAM-CL-TR-696.pdf&clen=940825&chunk=true)6.1和6.2章節，使用IMU來估測下一次做icp的初始狀態，以及使用速度、加速度對位置進行估測。使得每次icp的初始值都可以連續。結果為下圖，可以明顯看到跳動的問題改善。

>問題二:
>RMSE無法降低，準確度不夠。

解決: 發現調整`icp.setMaxCorrespondenceDistance`可以有效提升準確度。其意義為設定對應點的最大距離(m)。

>問題三:
>等待/tf導致跑完整個rosbag耗時。

解決: 由於測量儀器如光達，與車子坐標系之間的轉換是定值，因此先用`rostopic echo`將之間的轉換讀取出來，再把其值寫在程式當中，可以有效提升運算效率。

>問題四:
>第一筆`/lidar_points`沒讀到，導致解果少一筆資料。

解決: 原本使用兩個terminal分別開啟`rosbag`node和`sdc_mid_2`node。發現讀取地圖期間第一筆資料會沒讀到，因此使用`roslaunch`的方式來同時開啟，檔案如下。特別加入開啟`sdc_mid_2`node後等待12秒，讓地圖讀取完畢再撥放bag。再來是加入`--pause`讓rosbag 開啟後先暫停，等待手動撥放。以上兩步驟可以改善漏資料的問題。

<launch>
    <node name="sdc_mid" pkg="sdc_mid_2" type="lidar_subpub" launch-prefix ="gnome-terminal -x"/>
    <arg name="node_start_delay" default="12.0" /> 
    <param name="use_sim_time" value="true"/>
    <node pkg="rosbag" type="play" name="rosbag_play" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@' gnome-terminal -x" 
             args="--clock -r 0.1 --pause /home/stonelab510/catkin_ws/src/sdc_mid_2/sdc_localization_2_lite.bag"/>

</launch>

Contribution
---
本次作業為了提升結果的準確度，特別**使用imu角速度以及利用前一步的速度(後來想想應該要用odometry來做比較合理)來估測下一步的狀態初始值**。
因為所有資料都是離散的，因此使用`rectangular rule`來實作，雖然每筆資料之間的時間差(imu的取樣頻率是約100HZ)可能會造成誤差，但實做出來效果不錯。下圖為rectangular rule的示意圖。

Discussion
---
前一部分所使用的演算法為`Strapdown Inertial Nacigation`。姿態的估測為下式:
\begin{array}{l}
C(t + dt) = C(t)\left( {I + \frac{{\sin (\sigma )}}{\sigma }B + \frac{{1 - \cos (\sigma )}}{{{\sigma ^2}}}{B^2}} \right)
\end{array}

由於在計算過程中陀螺儀的值式由積分計算出來的，因此誤差會隨著時間累加，主要來自白噪音以及偏差值所造成。白噪音是一種隨機分布，其變異數回隨著時間的平方成正比。另一方面，偏差值事固定的誤差，因此與時間的一次成正比。量化的誤差除了來自離散的角速度轉成連續訊號也會造成誤差的產生，也會因為積分所導致。

位置、速度的估測為:
\begin{array}{l}
v(k + 1) = v(k) + dt(a(k) - g)\\
x(k + 1) = x(k) + dt \cdot v(k)
\end{array}

因為位置為加速度的二次積分，因此為偏移的主要原因。角速度的誤差也會在位置估測上造成誤差，因為旋轉矩陣C如果有一點誤差的話，會導致速度以及加速度從`body-frame`轉到`inertia frame`時產生偏差。這樣除了導致方向的誤差之外也會造成重力加速度無法對應到`inertia frame`的z軸上，微小的偏差使得g在x-y平面上會有分量。
雖然所產生的姿態誤差可能很小，但是隨著時間的累積下，所造成的位置誤差是很可觀的。
舉一個例子:
當姿態誤差約0.05(deg)，所造成重力加速度在xy方向的偏差約為0.0086(m/s^2)，經過30秒後，整體偏差高大7.7公尺，因此小角度的姿態偏差是不可以忽略的。