Алгоритм построения плоскости по облаку точек методом наименьших квадратов – довольно простой, но я не нашел готового алгоритма в Интернете, есть только общие рекомендации:

1. Написать параметрическое уравнение плоскости

2. Задать функцию, которую нужно минимизировать, в данном случае – сумму квадратов отклонений координат исходных точек от плоскости, аргументы функции - параметры уравнения плоскости

3. Найти локальный минимум этой функции, приравняв частные производные по ее аргументам нулю.

Направление действий – понятно, но к вычислительному алгоритму есть ряд требований:

1. Алгоритм должен корректно работать с любым набором исходных точек, и для любой ориентации плоскости в пространстве

2. Алгоритм должен обеспечивать приемлемую точность расчета при задании координат точек большими числами, например, если компактное облако точек, через которые требуется провести плоскость лежит далеко от начала координат. Или, характерный размер облака точек определяется миллиметрами, в то время, как само облако точек смещено от начала координат на десятки метров.

3. Алгоритм не должен рушиться и приводить к делению на ноль, или неопределенности типа ноль/ноль при любом расположении точек, например, если все точки лежат на одной прямой, или лежат симметрично на сфере, например, в вершинах тетраэдра.

Не даром, алгоритмы, которые используются в системах позиционирования ЧПУ, или в каких-то CAD-CAM системах часто бывают сертифицированы.

В итоге, пришлось самому разбираться. Я решил написать эту статью, т.к. в конце статьи будет готовый к применению алгоритм расчета. Надеюсь, кому-нибудь окажется полезно.

Задача такая – есть некоторое количество точек M(x,y,z) в трехмерном пространстве, нужно найти плоскость такую, чтобы сумма квадратов расстояний от точек M до этой плоскости была минимальной.

 1) Уравнение плоскости:

Здесь, вообще говоря, числа – любые, но обычно под ABC подразумевают единичный вектор нормали, а D – расстояние от начала координат до плоскости. Если D положительна, то начало координат лежит «над плоскостью» (вектор нормали направлен в сторону от начала координат), если D отрицательна - начало координат лежит «под плоскостью».

2) Расстояние от произвольной точки M(x,y,z) до плоскости вырежется формулой:

3) Потребуем выполнения условия:

4) Раскроем скобки для d² (разными способами, комбинируя разные слагаемые)

4) Предположим S(A,B,C,D) имеет локальный минимум. Найдём такие A,B,C,D, чтобы частные производные функции S по её аргументам A,B,C и D равнялись нулю.

5) Получилась система из четырёх уравнений с четырьмя неизвестными A,B,C,D (N - число точек):

6) В системе координат, связанной с центром масс точек M расстояние до начала координат
D = 0, поэтому хорошо бы сместить все исходные точки так, чтобы центр масс облака точек совпадал с началом координат, для этого следует вычислить центр масс Mo = (Σx/N, Σy/N, Σz/N), а новое облако точек будет: M’ = M−Mo (нужно выполнить это преобразование для каждой точки). Вектор нормали ABC останется прежним для такого преобразования координат точек. Чтобы избежать тривиального решения A=B=C=0 обычно вводится дополнительное условие нормировки |ABC| = 1

7) можно ввести обозначения для известных чисел XX, YY, ZZ, XY, YZ, ZX (их можно просто вычислить, просуммировав соответствующие выражения):

Перенос начала координат в центр масс как раз избавляет нас от операций с плавающей точкой с большими числами, которые могут привести к потере точности, если, например, характерный размер исходного облака точек определяется миллиметрами, в то время, как само облако точек смещено от начала координат на десятки метров.

8) Система уравнения принимает вид:

Я сначала встал в тупик на этом этапе, поскольку не знал, что с этой системой уравнений делать дальше. Компоненты вектора ABC не являются независимыми, они связаны некоторыми пропорциями. Я сначала подумал, что не сделаю большой ошибки, если вместо нулей подставлю справа столбец из некоторых достаточно малых чисел δ₁, δ₂ и δ₃ , таких, чтобы они были много меньше диагональных членов матрицы: |δ₁|+|δ₂|+ |δ₃| << XX+YY+ZZ, тогда система уравнений легко решается по точным формулам, а полученный таким образом вектор нормали придется нормировать на его дину, чтобы выполнить условие A²+B²+C² = 1.

Но, как оказалось, при неудачном выборе чисел δ₁, δ₂ и δ₃ нормировочная длина вектора может оказаться равной нулю. Это также можно обойти, сделав две циклических перестановки отличных друг от друга чисел δ₁, δ₂ и δ₃, и выбрать тот, из полученных тремя способами векторов ABC, у которого бы нормировочная длина была бы максимальна. Хотя такое решение дает довольно точный результат, особенно, если исходные точки хорошо ложатся на плоскость, но все равно это – тупиковое и не совсем точное решение.

9) Дальше я опишу, как нужно было бы действовать:

Правым множителем выступает матрица, составленная из известных чисел (назовем ее литерой F, чтобы не путать с введенными обозначениями).

Нужно найти собственные значения этой матрицы, и собственный вектор, тот, который соответствует минимальному из этих собственных значений. Этот вектор и будет являться вектором нормали к плоскости. Для нахождения собственных значений λ вообще говоря нужно решить характеристическое уравнение матрицы:

Здесь E – единичная матрица. Если раскрыть определитель, то получится кубическое уравнение для λ, корни которого на обязательно - вещественные числа.

Но нам повезло – наша матрица – симметричная. Для симметричной матрицы все собственные значения λ являются вещественными числами. Переобозначим компоненты матрицы F:

9) Для симметричной матрицы можно найти все собственные значения и собственные векторы без нахождения корней характеристического уравнения, а используя быстро сходящийся метод вращений. Здесь я нашел подробную инструкцию с доказательствами и примерами: Методы нахождения собственных значений и собственных векторов матриц

Для применения метода вращений нужно выполнить следующие шаги:

Шаг 1. Определить максимальный по абсолютной величине недиагональный компонент матрицы. У нас таких чисел всего три: f₁₂, f₁₃ и f₂₃, пусть это будет компонент f_kn (k<n)

Шаг 2. Рассчитать угол поворота вокруг одной оси, который бы исключил этот недиагональный компонент:

Здесь – единственное место, где следует проводить проверку на деление на ноль. Если значение скобок (f_kk-f_nn) близко к нулю, положим φ = π/2.

Шаг 3. Составить матрицу поворота U, заменяя в единичной матрице нули в позициях (k,n) и (n,k) на значения –sin(φ) и + sin(φ), соответственно, а также заменяя единицы в позициях (k,k) и (n,n) на значение cos(φ). Например, если k=1 и n =3, тогда:

Шаг 4. Вычислить новую матрицу F, как произведение трех матриц:

Далее следует повторить шаги с 1 по 4 до тех пор, пока матрица F не примет диагональный вид (все недиагональные компоненты – близки к нулю). Поскольку мы при каждой итерации довольно точно "угадываем" угол поворота φ, то трех-пяти итераций будет более чем достаточно. Матрицы поворота, U, которые получаются на шаге 2 мы будем условно нумеровать для каждой итерации, как U₁, U₂, U₃ и т. д.

Нам также понадобится некоторая результирующая матрица поворота, которая вычисляется, как последовательное произведение всех матриц поворота, полученных при каждой итерации:

Эту результирующую матрицу можно сразу вычислять при каждой итерации, задав начальное значение Ur = E (единичная матрица), и при каждой итерации просто модифицировать ее умножением справа на полученную в очередной итерации матрицу поворота, тогда сами матрицы U₁∙U₂∙U₃ ... можно не сохранять, как промежуточный результат.

Шаг 5. Полученная в последней итерации диагональная матрица F будет иметь следующий вид:

Здесь λ_n – собственные значения, а соответствующие столбцы результирующей матрицы Ur – собственные векторы исходной матрицы F.

Нужно найти наименьшее собственное значение λ_n, тогда соответствующий n-столбец матрицы Ur окажется искомым вектором ABC нормали к плоскости. Причем, поскольку матрица Ur получена, как произведение матриц поворота, то длина вектора ABC будет сразу равна единице, т.е. его не придется дополнительно нормировать.

 Шаг. 6 Осталось вычислить D по формуле:

Здесь нужно помнить, что Σx, Σy и Σz – это суммы координат исходных (несмещенных) точек, в системе координат, в которой мы вычисляли центр масс Mo в пункте 6).

Есть одно интересное замечание: если на шаге 5 выбрать не минимальное λ_n, а наоборот, максимальное, то полученный вектор будет являться вовсе не нормалью к плоскости, а направлением прямой, которая проведена через то же облако точек. Так что функция может быть универсальной, и годится как для аппроксимации плоскости, так и для аппроксимации прямой, проходящей через точки.

 Ниже пример функции, которая вычисляет вектор ABC по известным числам XX, YY, ZZ, XY, YZ, ZX, пример написан для ПЛК на языке из семейства МЭК61131-3, поэтому специально не вставлял никаких циклов типа while.

Я тестировал эту функцию на устойчивость к самым невероятным наборам входных данных, исходные точки вообще могут совпадать, или не определять плоскость, например, лежать симметрично на сфере, или на одной прямой, тогда плоскость может как-то произвольно сориентироваться в пространстве. Но, если есть хоть какой-то намек на то, что исходные точки определяют плоскость, то алгоритм четко находит решение. Единственная особенность, функция не делает разницы между решениями A, B, C и -A, -B, -C, и находит первое попавшееся из двух равнозначных решений. Но здесь нужно уже самостоятельно смотреть, исходя из других параметров прикладной задачи, в какую сторону должен быть направлен вектор нормали.