From 57ca1d9cdf49339f03e92cfdbc634f709c902249 Mon Sep 17 00:00:00 2001 From: Hunaid Hameed Date: Fri, 28 Jul 2023 15:54:21 +0200 Subject: [PATCH] convert from geodesic to map coords --- seerep_srv/seerep_core/src/core_project.cpp | 29 +++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/seerep_srv/seerep_core/src/core_project.cpp b/seerep_srv/seerep_core/src/core_project.cpp index dccbe26c0..9f4abd0fa 100644 --- a/seerep_srv/seerep_core/src/core_project.cpp +++ b/seerep_srv/seerep_core/src/core_project.cpp @@ -95,7 +95,36 @@ seerep_core_msgs::QueryResultProject CoreProject::getDataset(seerep_core_msgs::Q // is the query not in map frame? if (!query.inMapFrame) { + seerep_core_msgs::GeodeticCoordinates g = m_geodeticCoordinates.value(); + + // https://proj.org/en/9.2/operations/conversions/topocentric.html + // the proj pipeline has two steps, convert geodesic to cartesian coordinates + // then project to topocentric coordinates + std::string proj_pipeline = "step +proj=cartesian +ellps=" + g.coordinateSystem + + " \nstep +proj=topocentric +ellps" + g.coordinateSystem + + "+lon_0=" + std::to_string(g.longitude) + "lat_0=" + std::to_string(g.latitude) + + "h_0=" + std::to_string(g.altitude); + + // PJ_FWD: forward, PJ_IDENT: identity, PJ_INV: inverse + PJ_DIRECTION direction = PJ_FWD; + + seerep_core_msgs::Polygon2D transformed_polygon; + // perform an affine transform to transpose the query polygon to map frame + // the first argument of 0 is the thread context, the second is the pipeline string created above + auto to_topographic = proj_create(0, proj_pipeline.c_str()); + + // we traverse the polygon and apply the transform + for (seerep_core_msgs::Point2D p : query.polygon.value().vertices) + { + PJ_COORD c = proj_coord(p.get<0>(), p.get<1>(), NULL, NULL); + PJ_COORD t_coord = proj_trans(to_topographic, direction, c); + + seerep_core_msgs::Point2D transformed_p; + p.set<0>(t_coord.xy.x); + p.set<1>(t_coord.xy.y); + transformed_polygon.vertices.push_back(transformed_p); + } } result.dataOrInstanceUuids = m_coreDatasets->getData(m_coreTfs->transformQuery(query, m_frameId));