3D曲面上的TSP问题（一）：曲面上点集距离求解

3D曲面上，两点的距离求解不能采用欧式距离，而需要计算测地线距离。

代码使用CGAL 5.6.2 + OpenCV 4.11.0 版本实现

#include "cgal_utils.h"
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Surface_mesh_shortest_path.h>
#include <CGAL/boost/graph/graph_traits_Surface_mesh.h>// 将CGAL点转换为glm::dvec3
glm::dvec3 cgal_utils::cgalToGLM(const cgal_utils::Point_3& p) {return glm::dvec3(CGAL::to_double(p.x()),CGAL::to_double(p.y()),CGAL::to_double(p.z()));
}// 将glm::dvec3转换为CGAL点
cgal_utils::Point_3 cgal_utils::glmToCGAL(const glm::dvec3& p) {return cgal_utils::Point_3(p.x, p.y, p.z);
}// 将glm::dvec3转换为CGAL点
std::vector<cgal_utils::Point_3> cgal_utils::convertToCGALPoints(const std::vector<glm::dvec3>& points) {std::vector<Point_3> cgalPoints;cgalPoints.reserve(points.size());for (const auto& p : points) {cgalPoints.emplace_back(p.x, p.y, p.z);}return cgalPoints;
}// 计算凸包并返回Polyhedron
cgal_utils::Polyhedron_3 cgal_utils::computeConvexHullAsPolyhedron(const std::vector<glm::dvec3>& inputPoints) 
{// 转换点格式std::vector<cgal_utils::Point_3> points = convertToCGALPoints(inputPoints);// 计算凸包cgal_utils::Polyhedron_3 poly;CGAL::convex_hull_3(points.begin(), points.end(), poly);return poly;
}// 计算凸包并返回Surface_mesh
cgal_utils::Surface_mesh cgal_utils::computeConvexHullAsSurfaceMesh(const std::vector<glm::dvec3>& inputPoints) {// 转换点格式std::vector<cgal_utils::Point_3> points = convertToCGALPoints(inputPoints);// 计算凸包cgal_utils::Surface_mesh mesh;CGAL::convex_hull_3(points.begin(), points.end(), mesh);return mesh;
}// 保存Polyhedron到OFF文件
void cgal_utils::savePolyhedronToOFF(const cgal_utils::Polyhedron_3& poly, const std::string& filename) {std::ofstream out(filename);out << poly;out.close();
}// 保存Surface_mesh到OFF文件
void cgal_utils::saveSurfaceMeshToOFF(const cgal_utils::Surface_mesh& mesh, const std::string& filename) {std::ofstream out(filename);out << mesh;out.close();
}// 计算每个点到凸包的最近点
std::vector<glm::dvec3> cgal_utils::computeClosestPointsOnHull(const std::vector<glm::dvec3>& inputPoints,const cgal_utils::Surface_mesh& convexHull) {// 创建AABB树用于最近点查询typedef CGAL::AABB_face_graph_triangle_primitive<Surface_mesh> Primitive;typedef CGAL::AABB_traits<K, Primitive> Traits;typedef CGAL::AABB_tree<Traits> Tree;Tree tree(faces(convexHull).first, faces(convexHull).second, convexHull);tree.accelerate_distance_queries();std::vector<glm::dvec3> closestPoints;closestPoints.reserve(inputPoints.size());for (const auto& p : inputPoints) {Point_3 query = cgal_utils::glmToCGAL(p);Point_3 closest = tree.closest_point(query);if (isnan(closest.x()))std::cout << "nan" << std::endl;closestPoints.push_back(cgalToGLM(closest));}return closestPoints;
}// 计算测地线距离矩阵
cv::Mat cgal_utils::computeGeodesicDistances(const std::vector<glm::dvec3>& points,const cgal_utils::Surface_mesh& mesh) {// 创建距离矩阵
cv::Mat distanceMatrix(points.size(), points.size(), CV_64F);// 创建最短路径计算对象
typedef CGAL::Surface_mesh_shortest_path_traits<K, Surface_mesh> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Shortest_path;
typedef typename Shortest_path::Face_location              Face_location;typedef CGAL::AABB_face_graph_triangle_primitive<Surface_mesh>     AABB_face_graph_primitive;
typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>            AABB_face_graph_traits;
typedef CGAL::AABB_tree<AABB_face_graph_traits>                    AABB_tree;Shortest_path shortest_paths(mesh);auto checkLoc = [](Face_location& loc){if(std::isnan(loc.second[0]) || std::isinf(loc.second[0]))return false;return true;};// 为每个点找到最近的顶点、边或面
std::vector<Face_location> locations;
for (const auto& p : points) {cgal_utils::Point_3 query = cgal_utils::glmToCGAL(p);auto loc = shortest_paths.locate<AABB_face_graph_traits>(query);// 这里有一些困惑，会存在nan或inf值的问题，临时的解决方案是// 发现这样的值之后，让原来的点稍稍偏一点距离，再重新求一遍；// 这里为了以防万一，XYZ轴各试一遍，直到值正常了就不再试了if (!checkLoc(loc)){query = cgal_utils::glmToCGAL(p + glm::dvec3(0.001, 0, 0));loc = shortest_paths.locate<AABB_face_graph_traits>(query);if (!checkLoc(loc)){query = cgal_utils::glmToCGAL(p + glm::dvec3(0, 0.001, 0));loc = shortest_paths.locate<AABB_face_graph_traits>(query);if (!checkLoc(loc)){query = cgal_utils::glmToCGAL(p + glm::dvec3(0, 0, 0.001));loc = shortest_paths.locate<AABB_face_graph_traits>(query);}}}locations.push_back(loc);}// 计算所有点对之间的测地线距离for (size_t i = 0; i < points.size(); ++i) {shortest_paths.add_source_point(locations[i]);for (size_t j = 0; j < points.size(); ++j) {if (i == j) {distanceMatrix.at<double>(i, j) = 0.0;}else {double dist = CGAL::to_double(shortest_paths.shortest_distance_to_source_points(locations[j].first, locations[j].second).first);if (dist < 0)dist = 100000;distanceMatrix.at<double>(i, j) = dist;}}shortest_paths.clear();}return distanceMatrix;
}cv::Mat cgal_utils::computeConvexHullAndGeodesicDistances(const std::vector<glm::dvec3>& points)
{auto mesh = computeConvexHullAsSurfaceMesh(points);//saveSurfaceMeshToOFF(mesh, "convex_hull.off");auto nearestPts = computeClosestPointsOnHull(points, mesh);cv::Mat result = computeGeodesicDistances(nearestPts, mesh);return result;
}