GEOS 代码

//这段代码追要是把一条线变成五条线(分别向左向右偏移两次)
int GeneratLineFeature(sp<SemanticGroupFeature> &road_line_features,
                        sp<SemanticGroupFeature> &line_features)
{
    LOGD("GeneratLineFeature");
    int lane_count = 2; //写死车道
    int lane_width = 3.5; //默认车道宽度
    for(const auto& feature : (*road_line_features->features_))
    {  
        initGEOS(notice, log_and_exit);    
        GEOSSTRtree* tree = GEOSSTRtree_create(10);//创建STR树
        std::vector<GEOSGeometry*> treeGeoms;//GEOSGeometry代表空间几何对象，点线边等
        algorithm::ConvLLA2ENU lla2enu;
        if (feature->geometry_type_ == GeometryType::POLYLINE_TYPE)
        {
            if (feature->polyline_.pts_.size() < 4)
            {
                //当点位较少时需要另行判断
                GeneratSimplenessLine(feature->polyline_, lane_count, lane_width, line_features);
                continue;
            }
            std::vector<double> coords;//存储转换enu后的坐标（x,y分开存放）
            //填充 treeGeoms(点对象集合，enu下的xyz), 再遍历treeGeoms把每个点加入tree 填充coords(enu x,y分开存放)
            CreateTree(feature->polyline_, tree, treeGeoms, coords, lla2enu);
            
            int numPoints = static_cast<int>(coords.size()/2)-1;//-1去掉尾点
            GEOSCoordSequence * coordSeq = GEOSCoordSeq_create(numPoints, 2);
            for (int i = 0; i< numPoints; i++)
            {
                GEOSCoordSeq_setX(coordSeq, i, coords[2*i]);
                GEOSCoordSeq_setY(coordSeq, i, coords[2*i+1]);
            }
            //通过coordSeq坐标序列对象创建一个表示线GEOSGeometry(几何)对象
            GEOSGeometry *lineString = GEOSGeom_createLineString(coordSeq);
            // std::string line_str = GEOSGeomToWKT(lineString);
            // std::cout <<line_str.c_str() <<std::endl;
            //将原始线与偏移后的线都存储在line_features中
            GeneratLineFromWidth(lineString, line_features, tree, lla2enu, lane_count, lane_width);
            if(coordSeq != nullptr)
            {
                GEOSCoordSeq_destroy(coordSeq);
            }
            if (tree != nullptr)
            {
                GEOSSTRtree_destroy(tree);
            }
            for (const auto& g : treeGeoms) {
                GEOSGeom_destroy(g);
            }
        }
        finishGEOS();
    }
    return 0;
}

void CreateTree(DataExportPolyline<double> polyline_, 
                        GEOSSTRtree* tree, 
                        std::vector<GEOSGeometry*> &treeGeoms,
                        std::vector<double>& coords, 
                        algorithm::ConvLLA2ENU &lla2enu)
{
    geometry::Vec3d enuOrigin(polyline_.pts_[0].x_, 
                            polyline_.pts_[0].y_, 
                            polyline_.pts_[0].z_);//设置第一个点为enu的原点
    lla2enu.Reset(enuOrigin);//重置原点为enuOrigin
    for(size_t i = 1; i <= polyline_.pts_.size()-1;  i++)
    {
        DataExportPoint<double> point = polyline_.pts_[i];
        geometry::Vec3d inputPoint(point.x_, point.y_, point.z_);//lla
        geometry::Vec3d outPoint;
        lla2enu.Forward(inputPoint, outPoint);//转成enu
        coords.push_back(outPoint.x());
        coords.push_back(outPoint.y());
        //建树z需要赋值，否则后面查询找不到z值
        //创建一个用于存储坐标序列的 GEOSCoordSequence 对象（包含的点的数量，每个点的维度）
        GEOSCoordSequence * treeSeq = GEOSCoordSeq_create(1, 3);
        GEOSCoordSeq_setX(treeSeq, 0, outPoint.x());//设置坐标序列中索引为 0 的点的 x 坐标为 outPoint.x()
        GEOSCoordSeq_setY(treeSeq, 0, outPoint.y());
        GEOSCoordSeq_setZ(treeSeq, 0, outPoint.z());
        GEOSGeometry *tree_point = GEOSGeom_createPoint(treeSeq);//创建点对象函数，参数为GEOSCoordSequence对象，包含点信息
        treeGeoms.push_back(tree_point);
    }
    for (const auto& g : treeGeoms) {
        GEOSSTRtree_insert(tree, g, g);//第二个g为标识符，便于后续访问该几何对象
    }
}

void GeneratLineFromWidth(GEOSGeometry *lineString, //序列点集(x,y) 线几何特征
                        sp<SemanticGroupFeature> &line_features,//用于存储
                        GEOSSTRtree* tree, algorithm::ConvLLA2ENU lla2enu,
                        int lane_count,//2
                        int lane_width)//3.5
{
    AddLine(lineString, line_features, tree, lla2enu);//把lineString转成lla存储在line_features中
    for (int i = 0; i < lane_count; i++)
    {
        // 正值向右，负值向左
        double width = lane_width*(i+1);
        GEOSGeometry *offsetCurve_right = GEOSOffsetCurve(lineString, width, 8, GEOSBUF_JOIN_ROUND, GEOSBUF_CAP_ROUND);
        AddLine(offsetCurve_right, line_features, tree, lla2enu);

        GEOSGeometry *offsetCurve_left = GEOSOffsetCurve(lineString, -width, 8, GEOSBUF_JOIN_ROUND, GEOSBUF_CAP_ROUND);
        AddLine(offsetCurve_left, line_features, tree, lla2enu);

        GEOSGeom_destroy(offsetCurve_right);
        GEOSGeom_destroy(offsetCurve_left);
    }
}

void AddLine(GEOSGeometry *offsetCurve_line, sp<SemanticGroupFeature> &line_features,
            GEOSSTRtree* tree, algorithm::ConvLLA2ENU lla2enu)//tree 点(x,y,z)
{
    sp<SemanticFeature> s_feature = std::make_shared<SemanticFeature>();
    //从offsetCurve_line获取坐标序列。每个坐标序列都包含一系列的点坐标，并保存了这些点的维度信息
    const GEOSCoordSequence *coordSeq = GEOSGeom_getCoordSeq(offsetCurve_line);
    unsigned int numPoints = 0;
    GEOSCoordSeq_getSize(coordSeq, &numPoints);//获得序列中点的数量
    for (unsigned int i=0; i < numPoints; i++)
    {
        double x,y,z=0.0;
        GEOSCoordSeq_getX(coordSeq, i, &x);//获取序列点的x值，存储在变量x中
        GEOSCoordSeq_getY(coordSeq, i, &y);
        
        GEOSCoordSequence *pointSeq = GEOSCoordSeq_create(1, 3);
        GEOSCoordSeq_setX(pointSeq, 0, x);
        GEOSCoordSeq_setY(pointSeq, 0, y);
        // 设z为0，查询时保证能找到附近点
        GEOSCoordSeq_setZ(pointSeq, 0, 0);
        GEOSGeometry *point = GEOSGeom_createPoint(pointSeq);
        const GEOSGeometry *nearestPoint = GEOSSTRtree_nearest(tree, point);//找到与point最近的点
        const GEOSCoordSequence *nearest_coordSeq = GEOSGeom_getCoordSeq(nearestPoint);//获得最近点的坐标序列
        GEOSCoordSeq_getZ(nearest_coordSeq, 0, &z);//从坐标序列中获取索引为0的Z坐标，存储在z中
        //当前点x,y与最近点的z(认为他们两个近似，所以取最近点的Z)
        geometry::Vec3d inputPoint(x, y, z);
        geometry::Vec3d outPoint;
        // enu转回lla
        lla2enu.Reverse(inputPoint, outPoint);
        s_feature->polyline_.AddPoint(DataExportPoint<double>(outPoint.x(),outPoint.y(),outPoint.z()));
        GEOSGeom_destroy(point);
    }
    s_feature->geometry_type_ = GeometryType::POLYLINE_TYPE;
    line_features->AddSemanticFeature(s_feature);
}