TSPSolver.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot kit at edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TSPSolver.h"
 
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
 
#include <range/v3/algorithm/reverse.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/concat.hpp>
#include <range/v3/view/transform.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
namespace armarx::navigation::algorithms::tsp
{
 
    namespace detail
    {
        // Compute full distance matrix from points
        std::vector<std::vector<double>>
        buildDistanceMatrix(const std::vector<Eigen::Vector2f>& pts)
        {
            const size_t n = pts.size();
            std::vector<std::vector<double>> d(n, std::vector<double>(n, 0.0));
            for (size_t i = 0; i < n; ++i)
            {
                for (size_t j = 0; j < n; ++j)
                {
                    if (i == j)
                    {
                        continue;
                    }
                    d[i][j] = (pts.at(i) - pts.at(j)).norm();
                }
            }
            return d;
        }
 
        // Held-Karp (exact) TSP solver returning (cost, tour)
        // - start at node 0, return to node 0
        // - returns INF if impossible
        std::pair<double, std::vector<int>>
        heldKarpTsp(const std::vector<std::vector<double>>& d)
        {
            const int n = static_cast<int>(d.size());
            if (n == 0)
            {
                return {0.0, {}};
            }
            if (n == 1)
            {
                return {0.0, {0}};
            }
 
            const int FULL = 1 << n;
            const double INF = 1e100;
 
            // dp[mask][i] = minimal cost to reach subset `mask` ending at `i` (mask includes i)
            // only store masks where bit 0 is set (we always start from 0)
            std::vector<std::vector<double>> dp(FULL, std::vector<double>(n, INF));
            std::vector<std::vector<int>> parent(FULL, std::vector<int>(n, -1));
 
            dp[1 << 0][0] = 0.0; // start at node 0
 
            for (int mask = 0; mask < FULL; ++mask)
            {
                if (!(mask & 1))
                {
                    continue; // ensure start node is included
                }
                for (int u = 0; u < n; ++u)
                {
                    if (!(mask & (1 << u)))
                    {
                        continue;
                    }
                    double cur = dp[mask][u];
                    if (cur >= INF)
                    {
                        continue;
                    }
                    for (int v = 0; v < n; ++v)
                    {
                        if (mask & (1 << v))
                        {
                            continue; // v must be outside mask
                        }
                        int nxt = mask | (1 << v);
                        double nd = cur + d[u][v];
                        if (nd < dp[nxt][v])
                        {
                            dp[nxt][v] = nd;
                            parent[nxt][v] = u;
                        }
                    }
                }
            }
 
            // close the tour back to 0
            double best = INF;
            int best_last = -1;
            int final_mask = FULL - 1;
            for (int last = 1; last < n; ++last)
            {
                double cost = dp[final_mask][last] + d[last][0];
                if (cost < best)
                {
                    best = cost;
                    best_last = last;
                }
            }
 
            if (best_last == -1)
            {
                return {INF, {}};
            }
 
            // reconstruct path
            std::vector<int> tour;
            int mask = final_mask;
            int cur = best_last;
            while (cur != -1)
            {
                tour.push_back(cur);
                int p = parent[mask][cur];
                mask ^= (1 << cur);
                cur = p;
            }
 
            ranges::reverse(tour);
 
            // tour is 0 -> ... -> best_last -> 0, but we appended 0 at beginning: ensure return to 0
            tour.push_back(0);
 
            ARMARX_INFO << VAROUT(tour);
 
            // sanity check
            ARMARX_CHECK_EQUAL(tour.size(), static_cast<std::size_t>(n + 1));
 
            return {best, tour};
        }
    } // namespace detail
 
    TSPSolver::TSPSolver(const Parameters& params) : params_(params)
    {
        // Initialize with params if needed
    }
 
    std::optional<TSPSolver::Result>
    TSPSolver::solve(const std::vector<Eigen::Vector2f>& pts,
                     const Eigen::Vector2f& startingPosition) const
    {
        const std::vector<Eigen::Vector2f> startPt{startingPosition};
 
        // we internally add the starting position as the first point
        std::vector<Eigen::Vector2f> points =
            ranges::views::concat(startPt, pts) | ranges::to<std::vector<Eigen::Vector2f>>();
 
        auto d = detail::buildDistanceMatrix(points);
        auto [cost, tour] = detail::heldKarpTsp(d);
 
        if (cost >= 1e90)
        {
            ARMARX_ERROR << "TSP solver failed to find a solution.";
            return std::nullopt;
        }
 
        ARMARX_INFO << "TSP tour computed: " << tour;
        ARMARX_INFO << "TSP tour cost (millimeters): " << cost;
 
        // the first and last point are the starting position
        // we remove both from the orderIndices
 
        auto orderedPoints =
            ranges::views::transform(tour,
                                     [&points](std::int64_t idx) noexcept
                                     { return points.at(static_cast<std::size_t>(idx)); }) |
            ranges::to<std::vector<Eigen::Vector2f>>();
 
        // remove starting position from order indices
        orderedPoints.erase(orderedPoints.begin());
        // remove return to starting position
        orderedPoints.erase(orderedPoints.end() - 1);
 
        auto orderIndices = tour |
                            ranges::views::transform([](std::int64_t idx) noexcept
                                                     { return static_cast<std::size_t>(idx); }) |
                            ranges::to<std::vector<std::size_t>>();
 
        // remove starting position
        orderIndices.erase(orderIndices.begin());
        // remove return to starting position
        orderIndices.erase(orderIndices.end() - 1);
 
        // decrement indices by 1 to account for removed starting position
        for (auto& idx : orderIndices)
        {
            --idx;
        }
 
        // remove starting position from tour
        return Result{.orderedPoints = orderedPoints, .orderIndices = orderIndices};
    }
 
 
} // namespace armarx::navigation::algorithms::tsp