2021.07.03-基于软约束的轨迹优化-地图

参考内容：

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)”，

这个代码依赖的东西很少，可以先用这个试试看，不过不知道最后能不能用在grad_traj_optimization中。

 搞了一下午，没能搞出来，算了，先跳过SDF地图这一部分，接着往下进行。