WIP: Improvement in path planning
reorder_by_three_exchanges_with_segment_flipping() works, but it is excessively slow, with close to O(n^3) time complexity. Commited, but not used in production until sped up.
This commit is contained in:
bubnikv
parent
2b17e81f13
commit
d3ec53d9a6
@ -1333,6 +1333,82 @@ static inline std::pair<double, size_t> minimum_crossover_cost(
|
||||
return std::make_pair(cost_min, flip_min);
|
||||
}
|
||||
|
||||
static inline std::pair<double, size_t> minimum_crossover_cost(
|
||||
const std::vector<FlipEdge> &edges,
|
||||
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1,
|
||||
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2,
|
||||
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3,
|
||||
const std::pair<size_t, size_t> &span4, const ConnectionCost &cost4,
|
||||
const double cost_current)
|
||||
{
|
||||
auto connection_cost = [&edges](
|
||||
const std::pair<size_t, size_t> &span1, const ConnectionCost &cost1, bool reversed1, bool flipped1,
|
||||
const std::pair<size_t, size_t> &span2, const ConnectionCost &cost2, bool reversed2, bool flipped2,
|
||||
const std::pair<size_t, size_t> &span3, const ConnectionCost &cost3, bool reversed3, bool flipped3,
|
||||
const std::pair<size_t, size_t> &span4, const ConnectionCost &cost4, bool reversed4, bool flipped4) {
|
||||
auto first_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.first].p2 : edges[span.first].p1; };
|
||||
auto last_point = [&edges](const std::pair<size_t, size_t> &span, bool flipped) { return flipped ? edges[span.second - 1].p1 : edges[span.second - 1].p2; };
|
||||
auto point = [first_point, last_point](const std::pair<size_t, size_t> &span, bool start, bool flipped) { return start ? first_point(span, flipped) : last_point(span, flipped); };
|
||||
auto cost = [](const ConnectionCost &acost, bool flipped) {
|
||||
assert(acost.cost >= 0. && acost.cost_flipped >= 0.);
|
||||
return flipped ? acost.cost_flipped : acost.cost;
|
||||
};
|
||||
// Ignore reversed single segment spans.
|
||||
auto simple_span_ignore = [](const std::pair<size_t, size_t>& span, bool reversed) {
|
||||
return span.first + 1 == span.second && reversed;
|
||||
};
|
||||
assert(span1.first < span1.second);
|
||||
assert(span2.first < span2.second);
|
||||
assert(span3.first < span3.second);
|
||||
assert(span4.first < span4.second);
|
||||
return
|
||||
simple_span_ignore(span1, reversed1) || simple_span_ignore(span2, reversed2) || simple_span_ignore(span3, reversed3) || simple_span_ignore(span4, reversed4) ?
|
||||
// Don't perform unnecessary calculations simulating reversion of single segment spans.
|
||||
std::numeric_limits<double>::max() :
|
||||
// Calculate the cost of reverting chains and / or flipping segment orientations.
|
||||
cost(cost1, flipped1) + cost(cost2, flipped2) + cost(cost3, flipped3) + cost(cost4, flipped4) +
|
||||
(point(span2, ! reversed2, flipped2) - point(span1, reversed1, flipped1)).norm() +
|
||||
(point(span3, ! reversed3, flipped3) - point(span2, reversed2, flipped2)).norm() +
|
||||
(point(span4, ! reversed4, flipped4) - point(span3, reversed3, flipped3)).norm();
|
||||
};
|
||||
|
||||
#ifndef NDEBUG
|
||||
{
|
||||
double c = connection_cost(span1, cost1, false, false, span2, cost2, false, false, span3, cost3, false, false, span4, cost4, false, false);
|
||||
assert(std::abs(c - cost_current) < SCALED_EPSILON);
|
||||
}
|
||||
#endif /* NDEBUG */
|
||||
|
||||
double cost_min = cost_current;
|
||||
size_t flip_min = 0; // no flip, no improvement
|
||||
for (size_t i = 0; i < (1 << 8); ++ i) {
|
||||
// From the three combinations of 1,2,3 ordering, the other three are reversals of the first three.
|
||||
size_t permutation = 0;
|
||||
for (double c : {
|
||||
(i == 0) ? cost_current :
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, cost2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span1, cost1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, cost2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, cost3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, cost4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span3, cost3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span2, cost2, (i & 1) != 0, (i & (1 << 1)) != 0, span4, cost4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, cost3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span3, cost3, (i & 1) != 0, (i & (1 << 1)) != 0, span1, cost1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, cost2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0),
|
||||
connection_cost(span3, cost3, (i & 1) != 0, (i & (1 << 1)) != 0, span2, cost2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, cost1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, cost4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0)
|
||||
}) {
|
||||
if (c < cost_min) {
|
||||
cost_min = c;
|
||||
flip_min = i + (permutation << 8);
|
||||
}
|
||||
++ permutation;
|
||||
}
|
||||
}
|
||||
return std::make_pair(cost_min, flip_min);
|
||||
}
|
||||
|
||||
static inline void do_crossover(const std::vector<FlipEdge> &edges_in, std::vector<FlipEdge> &edges_out,
|
||||
const std::pair<size_t, size_t> &span1, const std::pair<size_t, size_t> &span2, const std::pair<size_t, size_t> &span3,
|
||||
size_t i)
|
||||
@ -1374,6 +1450,79 @@ static inline void do_crossover(const std::vector<FlipEdge> &edges_in, std::vect
|
||||
assert(edges_in.size() == edges_out.size());
|
||||
}
|
||||
|
||||
|
||||
static inline void do_crossover(const std::vector<FlipEdge> &edges_in, std::vector<FlipEdge> &edges_out,
|
||||
const std::pair<size_t, size_t> &span1, const std::pair<size_t, size_t> &span2, const std::pair<size_t, size_t> &span3, const std::pair<size_t, size_t> &span4,
|
||||
size_t i)
|
||||
{
|
||||
assert(edges_in.size() == edges_out.size());
|
||||
auto do_it = [&edges_in, &edges_out](
|
||||
const std::pair<size_t, size_t> &span1, bool reversed1, bool flipped1,
|
||||
const std::pair<size_t, size_t> &span2, bool reversed2, bool flipped2,
|
||||
const std::pair<size_t, size_t> &span3, bool reversed3, bool flipped3,
|
||||
const std::pair<size_t, size_t> &span4, bool reversed4, bool flipped4) {
|
||||
auto it_edges_out = edges_out.begin();
|
||||
auto copy_span = [&edges_in, &edges_out, &it_edges_out](std::pair<size_t, size_t> span, bool reversed, bool flipped) {
|
||||
assert(span.first < span.second);
|
||||
auto it = it_edges_out;
|
||||
if (reversed)
|
||||
std::reverse_copy(edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
|
||||
else
|
||||
std::copy (edges_in.begin() + span.first, edges_in.begin() + span.second, it_edges_out);
|
||||
it_edges_out += span.second - span.first;
|
||||
if (reversed != flipped) {
|
||||
for (; it != it_edges_out; ++ it)
|
||||
it->flip();
|
||||
}
|
||||
};
|
||||
copy_span(span1, reversed1, flipped1);
|
||||
copy_span(span2, reversed2, flipped2);
|
||||
copy_span(span3, reversed3, flipped3);
|
||||
copy_span(span4, reversed4, flipped4);
|
||||
};
|
||||
switch (i >> 8) {
|
||||
case 0:
|
||||
assert(i != 0); // otherwise it would be a no-op
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 1:
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 2:
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 3:
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 4:
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 5:
|
||||
do_it(span1, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span2, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 6:
|
||||
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span3, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 7:
|
||||
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span4, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 8:
|
||||
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span3, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 9:
|
||||
do_it(span2, (i & 1) != 0, (i & (1 << 1)) != 0, span4, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span3, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
case 10:
|
||||
do_it(span3, (i & 1) != 0, (i & (1 << 1)) != 0, span1, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span2, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
default:
|
||||
assert((i >> 8) == 11);
|
||||
do_it(span3, (i & 1) != 0, (i & (1 << 1)) != 0, span2, (i & (1 << 2)) != 0, (i & (1 << 3)) != 0, span1, (i & (1 << 4)) != 0, (i & (1 << 5)) != 0, span4, (i & (1 << 6)) != 0, (i & (1 << 7)) != 0);
|
||||
break;
|
||||
}
|
||||
assert(edges_in.size() == edges_out.size());
|
||||
}
|
||||
|
||||
static inline void reorder_by_two_exchanges_with_segment_flipping(std::vector<FlipEdge> &edges)
|
||||
{
|
||||
if (edges.size() < 2)
|
||||
@ -1448,6 +1597,90 @@ static inline void reorder_by_two_exchanges_with_segment_flipping(std::vector<Fl
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static inline void reorder_by_three_exchanges_with_segment_flipping(std::vector<FlipEdge> &edges)
|
||||
{
|
||||
if (edges.size() < 3) {
|
||||
reorder_by_two_exchanges_with_segment_flipping(edges);
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<ConnectionCost> connections(edges.size());
|
||||
std::vector<FlipEdge> edges_tmp(edges);
|
||||
std::vector<std::pair<double, size_t>> connection_lengths(edges.size() - 1, std::pair<double, size_t>(0., 0));
|
||||
std::vector<char> connection_tried(edges.size(), false);
|
||||
for (size_t iter = 0; iter < edges.size(); ++ iter) {
|
||||
// Initialize connection costs and connection lengths.
|
||||
for (size_t i = 1; i < edges.size(); ++ i) {
|
||||
const FlipEdge &e1 = edges[i - 1];
|
||||
const FlipEdge &e2 = edges[i];
|
||||
ConnectionCost &c = connections[i];
|
||||
c = connections[i - 1];
|
||||
double l = (e2.p1 - e1.p2).norm();
|
||||
c.cost += l;
|
||||
c.cost_flipped += (e2.p2 - e1.p1).norm();
|
||||
connection_lengths[i - 1] = std::make_pair(l, i);
|
||||
}
|
||||
std::sort(connection_lengths.begin(), connection_lengths.end(), [](const std::pair<double, size_t> &l, const std::pair<double, size_t> &r) { return l.first > r.first; });
|
||||
std::fill(connection_tried.begin(), connection_tried.end(), false);
|
||||
size_t crossover1_pos_final = std::numeric_limits<size_t>::max();
|
||||
size_t crossover2_pos_final = std::numeric_limits<size_t>::max();
|
||||
size_t crossover3_pos_final = std::numeric_limits<size_t>::max();
|
||||
size_t crossover_flip_final = 0;
|
||||
for (const std::pair<double, size_t> &first_crossover_candidate : connection_lengths) {
|
||||
double longest_connection_length = first_crossover_candidate.first;
|
||||
size_t longest_connection_idx = first_crossover_candidate.second;
|
||||
connection_tried[longest_connection_idx] = true;
|
||||
// Find the second crossover connection with the lowest total chain cost.
|
||||
size_t crossover_pos_min = std::numeric_limits<size_t>::max();
|
||||
double crossover_cost_min = connections.back().cost;
|
||||
for (size_t j = 1; j < connections.size(); ++ j)
|
||||
if (! connection_tried[j]) {
|
||||
for (size_t k = j + 1; k < connections.size(); ++ k)
|
||||
if (! connection_tried[k]) {
|
||||
size_t a = longest_connection_idx;
|
||||
size_t b = j;
|
||||
size_t c = k;
|
||||
if (a > c)
|
||||
std::swap(a, c);
|
||||
if (a > b)
|
||||
std::swap(a, b);
|
||||
if (b > c)
|
||||
std::swap(b, c);
|
||||
std::pair<double, size_t> cost_and_flip = minimum_crossover_cost(edges,
|
||||
std::make_pair(size_t(0), a), connections[a - 1], std::make_pair(a, b), connections[b - 1] - connections[a],
|
||||
std::make_pair(b, c), connections[c - 1] - connections[b], std::make_pair(c, edges.size()), connections.back() - connections[c],
|
||||
connections.back().cost);
|
||||
if (cost_and_flip.second > 0 && cost_and_flip.first < crossover_cost_min) {
|
||||
crossover_cost_min = cost_and_flip.first;
|
||||
crossover1_pos_final = a;
|
||||
crossover2_pos_final = b;
|
||||
crossover3_pos_final = c;
|
||||
crossover_flip_final = cost_and_flip.second;
|
||||
assert(crossover_cost_min < connections.back().cost + EPSILON);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (crossover_flip_final > 0) {
|
||||
// The cost of the chain with the proposed two crossovers has a lower total cost than the current chain. Apply the crossover.
|
||||
break;
|
||||
} else {
|
||||
// Continue with another long candidate edge.
|
||||
}
|
||||
}
|
||||
if (crossover_flip_final > 0) {
|
||||
// Pair of cross over positions and flip / reverse constellation has been found, which improves the total cost of the connection.
|
||||
// Perform a crossover.
|
||||
do_crossover(edges, edges_tmp, std::make_pair(size_t(0), crossover1_pos_final), std::make_pair(crossover1_pos_final, crossover2_pos_final),
|
||||
std::make_pair(crossover2_pos_final, crossover3_pos_final), std::make_pair(crossover3_pos_final, edges.size()), crossover_flip_final);
|
||||
edges.swap(edges_tmp);
|
||||
} else {
|
||||
// No valid pair of cross over positions was found improving the total cost. Giving up.
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Flip the sequences of polylines to lower the total length of connecting lines.
|
||||
static inline void improve_ordering_by_two_exchanges_with_segment_flipping(Polylines &polylines, bool fixed_start)
|
||||
{
|
||||
@ -1471,7 +1704,11 @@ static inline void improve_ordering_by_two_exchanges_with_segment_flipping(Polyl
|
||||
edges.reserve(polylines.size());
|
||||
std::transform(polylines.begin(), polylines.end(), std::back_inserter(edges),
|
||||
[&polylines](const Polyline &pl){ return FlipEdge(pl.first_point().cast<double>(), pl.last_point().cast<double>(), &pl - polylines.data()); });
|
||||
#if 1
|
||||
reorder_by_two_exchanges_with_segment_flipping(edges);
|
||||
#else
|
||||
reorder_by_three_exchanges_with_segment_flipping(edges);
|
||||
#endif
|
||||
Polylines out;
|
||||
out.reserve(polylines.size());
|
||||
for (const FlipEdge &edge : edges) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user