Geos库学习之(三)——几何对象空间关系_geos空间关系计算-CSDN博客
//这段代码追要是把一条线变成五条线(分别向左向右偏移两次)
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);
}