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;
}