Оптическая система контроля состояния качества головки рельса

Вообщем, есть данные с оптического сканера, который сканирует внутреннюю часть рельса, а именно, координаты x и z сечения рельса. Есть прога, которая позволяет отфильтровать различные левые значения в результате зашумления при проезде, также с её помощью можно определить некоторые интересующие нас точки на профиле рельса и сравнить его с профилем рельса, который должен быть по ГОСТам. В данной проге используется МНК, для приближения экспериментального профиля к аналитическому. Требуется написать прогу, которая вместо этого метод использует метод ICP (итеративный алгоритм ближайших точек). метод уже есть в инете, просто нужно в нем разобраться и применить непосредственно к нашим данным