# Localization Midterm FAQ

1. How to find initial guess?

// find the initial orientation
if (!initialied) {
    pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> first_icp;
    float yaw, min_yaw, min_score = std::numeric_limits<float>::max();
    Eigen::Matrix4f min_pose(Eigen::Matrix4f::Identity());

    for (yaw = 0; yaw < (M_PI * 2); yaw += 0.6) {
        Eigen::Translation3f init_translation(gps_point.x, gps_point.y, gps_point.z);
        Eigen::AngleAxisf init_rotation_z(yaw, Eigen::Vector3f::UnitZ());
        init_guess = (init_translation * init_rotation_z).matrix();

        //... 
        //Do ICP here
        //...
            
        double score = first_icp.getFitnessScore(0.5);
        if (score < min_score) {
            min_score = score;
            min_pose = first_icp.getFinalTransformation();
            ROS_INFO("Update best pose");
        }
    }
    // set initial guess
    init_guess = min_pose;
    initialied = true;
}

---
2. 使用了nuscenes.rviz的rviz卻未能顯示出當下的地圖？
- 載入的地圖部分離原點太遠所以看不到，
可以先把map的point size 調到10 ，再用滑鼠中鍵拖過去；
我們給的設定檔是視角會跟著localization 的結果跑（從rviz 右邊的view->target frame設定），
這邊的target frame 是localization 的output，所以只要localization 有跑起來他就會自動跳到定位所在的地方。
可以參考：http://wiki.ros.org/rviz/UserGuide

---
3. 無法進入docker
- ```--gpus``` all 拿掉應該就可以進container 了

---
4. 讀進來的lidar座標 (scan_points 這個參數) 是已經轉換到world座標、相對於車子的座標還是相對於lidar機器的座標?
5. GPS讀到的座標是車子的座標還是lidar機器的座標?

- 用rostopic echo /[topic_name] --noarr 可以看他的header，frame_id 即是他的座標系。在此次為lidar 座標系。
- 用一樣的方法即可察看。（用icp 算出來的應為world->lidar 的座標轉換，sample code 有提供把結果轉為world->car 的部份，同學可以參考）
---

6. Easy case剛開始可以找到正確的位置，但到後面的定位會整個偏掉.
- 在PCL ICP設定中有一個重要的參數，setMaxCorrespondenceDistance(), 意思是在做match時，在align階段會忽略超過Threshold的correspondence，這個如果不考量的話，點雲容易被誤差大點的拉歪。
- Youtube圖: [28:26] https://youtu.be/ktRqKxddjJk?t=1706
- PCL Reference: https://pointclouds.org/documentation/classpcl_1_1_registration.html#a65596dcc3cb5d2647857226fb3d999a5

---
7. 如果看起來不會發散的話，要提升精准度可以嘗試：

- 第一個initial guess 猜的更準。
- 把兩個epsilon 再調小，maxiteration 調大。
- 另外把getFitnessScore() 印出來，可以觀察ICP match 的情況，一般 <0.2 就是match 的不錯了。
---