Я думаю, ваша первоначальная идея великолепна. Я бы добавил / изменил две вещи:
1) Я бы добавил порог расстояния в ваш алгоритм: только когда вы проверяете текущую точку на некотором минимальном расстоянии от вашей последней «хорошей» точки, вы должны даже протестировать ее. В зависимости от источника данных вашего пути (возможно, от магнитного трекера?), Стационарность может плохо отражаться в ваших исходных данных из-за шума измерений. Это может привести к относительно большим изменениям направления в очень маленькой области, которые по сути бессмысленны.
2) Когда вы обнаружите достаточно большое изменение, не добавляйте текущую проверенную точку (как вы предложили), а предыдущую. В противном случае вы можете искажать путь. Пример (в 2D): путь, состоящий из (0,0) -> (1,0) -> (2,0) -> (3,0) -> (4,0) -> (5,5 ) будет в конечном итоге как (0,0) -> (5,5), используя ваши методы, что я не считаю хорошим представлением пути. Лучше было бы (0, 0) -> (4,0) -> (5,5).