參考內容：

grad_traj_optimization

motion_primitive_library

今日計劃內容：

1.載入柵格地圖（不管是txt、yaml、或者其他形式，都沒有啥區別），opencv存成圖片供顯示

2.柵格地圖轉換成sdf地圖

3.基於軟約束的軌跡優化開源專案學習

4.總結凸優化求解器OOQP的使用，以及如何建模，舉一反三。

一、載入柵格地圖

基本過程是：

1.讀取本地地圖，可以是txt，也可以是Yaml等格式，存放到一個比如二維指標陣列中data[][]。

2.通過智慧指標，傳遞給opencv繪圖類中。

    /*******************************讀取柵格地圖********************************/
    std::string mapdir = "/home/gao/workspace/planner/Astar/assets"; 
    //智慧指標
    boost::shared_ptr<GridMap> map_util;
    map_util.reset(new GridMap);
    map_util->readGrid2DData(mapdir + "/grid2d.txt");

    /*******************************opencv顯示********************************/
    OpenCVDrawing opencv_drawing(map_util);
    //show obstacles
    opencv_drawing.drawPoints(map_util->getCloud(), black);
    opencv_drawing.drawPotential(magenta, cyan);
    //show the plot
    std::string name = "test1";
    opencv_drawing.show(name);
    opencv_drawing.save(name + ".jpg");
 

3.繪圖有兩種方式

在opencv繪圖類建構函式中，宣告一個cv::Mat，大小根跟柵格地圖一樣。遍歷柵格地圖，將障礙物存到vector中，作為引數傳給函式drawPoints()。

繪製畫素點有兩種方式，一種是繪製一個矩形，另一種是直接在畫素上進行賦值。兩者效果相同。

    //Draw points
    void drawPoints(const std::vector<CTC::PolyPoint>& pts, cv::Scalar color, int line_width = 1) {
        for(const auto& it : pts) {
            cv::Point pt(it.x, it.y);
            cv::rectangle(img_, pt, pt, color, line_width); //pt, pt才是對的,與畫素操作效果一樣
        }
    }

    //Draw points畫素操作
    void drawPoints1(const std::vector<CTC::PolyPoint>& pts, cv::Scalar color, int line_width = 1) {
        for(const auto& it : pts) {
            cv::Point pt(it.x, it.y);
            img_.at<cv::Vec<uchar, 3>>(pt.y, pt.x) = cv::Vec<uchar, 3>(0, 0, 0);
        }
    }
 

二、柵格地圖轉換成距離場地圖

這裡就有點難搞了，grad_traj_optimization中，採用std_tools生成signed distance field，但是std_tools程式碼有點多啊，不太好移植。

在motion_primitive_library有提到“ To add a soft constraint based on the distance towards obstacles, one technique is to use the artificial potential field (APF)”，

這個程式碼依賴的東西很少，可以先用這個試試看，不過不知道最後能不能用在

搞了一下午，沒能搞出來，算了，先跳過SDF地圖這一部分，接著往下進行。