2

I have a set of (x, y, z) points for which I need to find the plane that best fits them. A plane is defined by its coefficients as:

a*x + b*y + c*z + d = 0

or equivalently:

A*X +B*y + C = z

The second equation is just a re-write of the first.

I'm using the method developed in this gist, which is a translation to Python from the Matlab code given in this answer. The method finds the coefficients to define the plane equation that best fits the set of points.

The issue is that I am able to come up with a set of coefficients that gives a better fit to that set of points.

To define "better", I calculate the sum of absolute distances of each point to the given plane, following the math given here. A smaller value, means a "better" fit, since the points are then on average closer to the plane.

The MWE is below. As can be seen, the hand-picked coefficients result in a smaller sum of absolute distance values (~155.89), than using the "best" coefficients found by the method described above (~158.78).

What am I missing here?


MWE

import numpy as np
import scipy.linalg


def sum_dist_2_plane(x, y, z, a, b, c, d):
    """
    Sum of the absolute values of the distances to a plane, given by the
    a,b,c,d coefficients, for the set of points defined by x,y,z.
    """
    return np.sum(abs(a*x + b*y + c*z + d)/np.sqrt(a**2+b**2+c**2), axis=0)


# Some xyz points.
xyz = np.array([[1.1724546888698482, 0.67037911349217505, 1.6014525241637045], [2.0029440384631063, 1.2163076402918147, -1.1082409593302032], [-0.87863180025363918, 1.261853987259635, 1.1598532675831237], [0.42789396045777467, 0.67325845732274703, 1.1421266649135475], [1.366142552248496, 1.0959456367043121, -1.6046393305927751], [-2.1595534005011485, -2.2582441035518794, -1.0663372184011806], [2.1104543583371633, -2.3711560770628917, 0.33077589412150843], [1.1974640975387107, 1.2100068141421523, 0.71395322259985505], [0.44492797840962123, 0.51098686422493145, 0.23383900276620295], [-2.0810094204638281, -2.11327958929372, -1.0758230448163033], [1.1655230345226737, 2.3777304002844968, -1.5663228128649394], [0.90952208156596781, 0.84978064084217519, 1.5986081506274985], [1.2951624720758836, 1.2231899029278033, 1.6154291293114866], [0.97545563477882025, 1.1844143994262264, 0.25292733170194026], [2.0281659385206012, 1.3370146330231019, 1.1961575550766028], [-1.9843445684092424, -0.012247402159192651, -2.0732736152121092], [1.0852175044560746, 1.8083916604163963, 0.27402181385868829], [-0.97983337631837208, 1.1032503818628847, 1.1579341604311182], [2.5033961310304029, 1.5628354191569325, -0.60785250636200061], [0.84123393662217383, 1.6169587554844618, -0.66116704633280676], [-1.8572657771039134, 0.043103553120073364, -2.0779545355975415], [2.6979128603518787, 1.70987170366249, -0.59306759275995091], [1.898614831265683, -2.9411794973775129, 1.7095862940118209], [0.81052668401212824, 0.89107411631439926, 1.597589407046101], [-2.0466083174114331, 0.14841369250699468, -1.120794708199135], [2.7004384737959648, 1.3616954868011328, 1.2294957766312749], [2.5373220833750385, 1.7067484497548233, 0.32345763726774379], [0.42025310188487158, 0.25762913945011717, -2.5899822318304473], [1.0425582222020597, 1.2902156453507225, 1.1638276333984123], [1.8492329386150801, 1.369745208770941, -1.1101559957041474], [-1.9685282554587256, -0.053725287173628226, 0.26827797508054374], [2.1798881190450285, 1.2454661605758286, -1.5732113885771071], [2.097212096433736, -2.9271738140601462, -0.56568133063870363], [-4.0108387171254396, -0.95559594599890008, 1.7588521192455815], [1.1558287640906737, 0.84330421357278096, 1.1565989504480143], [-2.9571643443632118, -2.847346163285049, 1.3087401683271338], [1.8592900784537116, 1.3952561066549967, 0.28365423946831214], [-3.4841441062982867, -3.0501496018162109, -0.48161393173162992], [2.5524429115550777, 0.62723764313314334, 0.29882336571990464], [2.2267279436912251, -3.8561674586606758, 1.3393813829669483], [2.1214758016437449, -0.20203416631090113, -1.5903243997743601], [0.14882165322179747, 0.4127883227210779, 0.23115527212661391], [1.2042041122995621, 1.2013226392201846, -0.2014020012510187], [-0.91807770884292583, 1.1176994160488214, -2.5723612427329385], [1.910565457302241, 1.1857852625952567, -1.5853233609652335], [1.0660312416826301, 1.3594393638452948, 0.71483235729161265], [0.65109075860726373, 0.58395151990229632, 1.590486638605114], [2.0967121651174518, 3.5121496638531586, 0.85481080660772335], [1.1484000297535542, 0.93256813649663772, 0.25125672956252743], [-1.7670514601312102, 0.17479726844255272, 0.26097336908379276], [-0.38814151285133675, -1.36837872393391, -2.0916940966530149], [1.5825758742579219, -0.34854211056693962, 0.2556641250097158], [2.586881293405797, -4.371974479474976, -2.3458559556297445], [0.22496107684878977, 0.26917053206799602, -0.69280100767942088], [-0.92198332953292639, 5.3103622894708327, 1.4344469946544294], [1.5669967464035819, -0.13527817891479368, 1.6081806927677107], [-0.56872000311273319, -1.9823395333139691, -2.5517609300755879], [-3.7708737466313824, -3.2863308845331081, 1.3928734104180975], [0.26086111146896701, 0.91063726352187491, -2.1025221562973897], [4.3490818342473947, 1.7969605233982313, -0.94470942930075807], [0.8202509554992351, 1.6178074457637883, -0.66148472916848533], [-1.5947972211483237, 0.18933818654144918, -0.20453683465790107], [0.9736103155058905, 1.4905334895713331, -2.0806647444063202], [1.2838541958241105, 2.0842224244281931, -0.17045822168000058], [3.7985716232291624, 2.5292902540646183, -0.022070946178700979], [1.175697191763003, 0.70063646974704663, 0.24808027552254686], [1.7834118390535998, 1.2937296781793448, -0.1818232448888395], [1.1281441478154344, 0.89641394438231292, 1.6040641573676311], [-2.0118889302553362, 2.7916846393274373, -0.57683324778643197], [-0.5995803308341846, -2.2434949940054554, 0.2835440401850704], [0.32077033536702831, -0.95844872063257081, -1.6245015133016167], [0.81357199339193753, 1.5540883407880133, -0.19956720143058249], [0.62611590692268004, 2.5129849486626958, -0.62767513959140331], [1.3018663649626585, 0.92514176013041427, 0.71042211390030729], [-0.72715254964437737, -2.3705643250823436, -0.63320562968051775], [1.9172742234794142, -2.8680592171367834, -1.9965843559235594], [-0.7108415762295921, -2.2783943434144658, -0.63767826146936812], [1.968546542650037, -2.8305910089272146, -0.11154135958968681], [-3.1492524087194655, -2.8503098024243823, -0.049957063615551078], [-4.0600431110777313, -0.97891479243488955, -0.055962425569617835], [-3.3752702254780629, 5.7587998072406652, 2.0459797674238658], [-1.9855135921592455, 2.7466682542750638, -0.58034791274582886], [2.033073141968945, 1.5208650449610079, -0.16592183863411947], [-1.0379089220195949, -4.7336396164389383, 0.0045652508195388464], [0.059579198580756186, 0.50654688886459498, -0.69144595015375643], [2.1785293390435458, -2.67576518666927, -2.4787451249989232], [2.1096278381494935, -0.41668256763302775, -2.5482230530414327], [2.898772426390924, 1.9762337520130302, 1.2619960149795091], [0.95620776766155502, 1.4639884373148864, -0.19976180368861662], [0.78751831482788348, 1.6888070662998231, -1.1280318812973462], [0.75574071441925506, -0.89893698883953688, -0.21651308186821439], [-0.26825101547751962, -3.4496728096007274, 1.7066486428460195], [1.6690385240329706, -0.49893224975237227, -0.66401176702524367], [-0.28877792353045606, 1.5139628395303639, 0.25314013342428154], [0.33435105972001761, 0.72567663189581422, -2.5862147225048417], [-0.29757422904759573, 1.5866751937867298, -0.6682501010682671], [2.7581055173587461, -3.973585217996157, 0.0036824743223959899], [-3.4344275379769509, -3.089933175898083, 0.44457796620464052], [-2.9394415977285413, -2.6122275577950083, 1.2944549102942418], [2.0038460695984823, 1.515512638618338, -1.5731231727332897], [2.206216953170296, 1.4688891052013793, -1.5661966567970254], [-1.035208468220836, 4.4666436487176657, 0.89858770640569929], [-2.0039938640838546, 0.24894412179006209, -1.1220951191237916], [-3.9104727661324539, -0.70689702779279451, 1.2978242803460915], [1.7290487193475563, 1.2850859351795931, -0.18395259620439219], [1.1198244545179541, 1.7335817969585154, -0.18776435816536718], [0.32239533364835676, 0.2896168073626299, -1.1602117002106667], [0.36649393980823192, 0.28244286109766281, -0.69190114531475189], [0.71629324271161154, 0.62574841994964003, 1.1448784055936088], [-0.65109499789331204, -1.3933343864454197, -2.0884024350786063], [0.97046822380567643, 1.5321191441287463, -0.19744980702830617], [-0.9585141324426697, 1.3494884330155692, 1.610936445675776], [0.9615111008482673, 2.4535668843530907, -1.0939899554364985], [-1.0667872216702354, 0.9585914740866075, 1.6038639420443772], [1.8021244106955299, 1.1320598433704154, 1.1820726259869971], [-0.060098920604716666, 0.46839599864404674, 2.0277692055269654], [0.1721690681247055, 0.33837718694053642, 1.137078044079125], [-1.5964760388322969, 0.29775223476696611, 1.1626558382504655], [2.233093222044507, -2.8349614127699461, 0.36052101139762271], [1.9257633093026034, -2.5325763598899247, -1.5360887301240496], [1.116293873468281, 0.82698434754975214, -2.5739062165349651], [1.1781306304855363, 0.67917370389645249, 1.6017135739225736], [-1.8600651472693519, 0.078727875114422086, 1.6184578422253679], [-1.43994317003447, 0.13431327308359137, 2.0472930703748276], [0.84521838040660946, 0.63970047924770745, -2.100345751420285], [1.7661749989776647, -0.37651847162651875, -2.0797840873592222], [0.83547092354865804, 1.7219104152802622, 0.2661115369175846], [1.8300570222025725, -0.28592323411250137, 1.6180934388285593], [-0.62076647836845089, -0.99191053757063119, -1.1486388713745725], [-1.6239006006253158, 0.41366361326031414, 0.2574990624750626], [0.89195815704237569, 2.2004172385784, -0.17400231396826626], [0.36791088305589931, 0.36096348396301231, -2.5897662606427687], [0.073648763901347059, 0.19675260582587464, -2.1107265203482299], [2.161140531872539, -2.842373820387067, 0.35775402140617274], [-2.0416416353442859, -4.4051625504298446, 0.0054589213454931951], [-2.0525396585901774, 3.6758248479033888, -2.4231570023949089], [-0.96441167578601306, -4.6667609706070516, -0.0032107139968431397], [-1.8689820843196163, 0.021432805852950151, 0.26440433366338567], [-0.15613351765730205, -1.0964152703770347, 1.5952653951331826], [-0.91084152695600051, 1.2388514346844914, 1.1598544561959656], [0.94699177145572266, 1.2276340276860185, 2.0505581774713733], [-0.8929399989505632, 1.2806485400811793, -0.20595242802870217], [1.2023125342023806, 2.3477287603163717, -1.5668539565738087], [1.1651535046949058, 1.3836371788871575, 0.26217241277176129], [-1.0929407572158512, 1.3887078738113698, -0.19910861560325088], [-0.76452840903206265, 1.4237410113821392, -1.6090659495628117], [-1.5594385646555604, 0.1455415355638911, 1.1607640518832483], [-0.59734981961340872, -1.2800366176149909, 1.6032259368271653], [1.2325774703556955, 0.80804053623212702, 0.25109224401040819], [1.177240124012167, 0.90163100927998241, -1.1405108476689563]])
x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]

# Best-fit linear plane, for the Eq: z = a*x + b*y + c.
# See: https://gist.github.com/amroamroamro/1db8d69b4b65e8bc66a6
A = np.c_[x, y, np.ones(xyz.shape[0])]
C, _, _, _ = scipy.linalg.lstsq(A, z)

# Coefficients in the form: a*x + b*y + c*z + d = 0.
a, b, c, d = C[0], C[1], -1., C[2]

# Sum of absolute distances of each point to this plane.
print sum_dist_2_plane(x, y, z, a, b, c, d)

# Hand-picked coefficients.
a, b, c, d = 0.28, -0.14, 0.95, 0.

# Sum of absolute distances of each point to this plane.
print sum_dist_2_plane(x, y, z, a, b, c, d)
Community
  • 1
  • 1
Gabriel
  • 40,504
  • 73
  • 230
  • 404

2 Answers2

9

The formula for the plane can be written as

z_plane = a*x + b*y + d

The vertical z-distance from the point to the plane is given by

|z_plane - z| = |a*x + b*y + d - z|

scipy.linalg.lstsq minimizes the square of the sum of these distances.

def zerror(x, y, z, a, b, d):
    return (((a*x + b*y + d) - z)**2).sum()

Indeed, the parameters returned by scipy.linalg.lstsq yield a smaller zerror than the hand-picked values:

In [113]: zerror(x, y, z, C[0], C[1], C[2])
Out[113]: 245.03516402045813

In [114]: zerror(x, y, z, 0.28, -0.14, 0.)
Out[114]: 323.81785779708787

The formula

enter image description here

gives the perpendicular distance between the point (x_0, y_0, z_0) and the plane, ax + by + cz + d = 0.


You could minimize the perpendicular distance to the plane using scipy.optimize.minimize (see minimize_perp_distance below).

import math
import numpy as np
import scipy.optimize as optimize
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d.axes3d as axes3d
np.random.seed(2016)

mean = np.array([0.0,0.0,0.0])
cov = np.array([[1.0,-0.5,0.8], [-0.5,1.1,0.0], [0.8,0.0,1.0]])
xyz = np.random.multivariate_normal(mean, cov, 50)
x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]

def minimize_z_error(x, y, z):
    # Best-fit linear plane, for the Eq: z = a*x + b*y + c.
    # See: https://gist.github.com/amroamroamro/1db8d69b4b65e8bc66a6
    A = np.c_[x, y, np.ones(x.shape)]
    C, resid, rank, singular_values = np.linalg.lstsq(A, z)

    # Coefficients in the form: a*x + b*y + c*z + d = 0.
    return C[0], C[1], -1., C[2]

def minimize_perp_distance(x, y, z):
    def model(params, xyz):
        a, b, c, d = params
        x, y, z = xyz
        length_squared = a**2 + b**2 + c**2
        return ((a * x + b * y + c * z + d) ** 2 / length_squared).sum() 

    def unit_length(params):
        a, b, c, d = params
        return a**2 + b**2 + c**2 - 1

    # constrain the vector perpendicular to the plane be of unit length
    cons = ({'type':'eq', 'fun': unit_length})
    sol = optimize.minimize(model, initial_guess, args=[x, y, z], constraints=cons)
    return tuple(sol.x)

initial_guess = 0.28, -0.14, 0.95, 0.
vert_params = minimize_z_error(x, y, z)
perp_params = minimize_perp_distance(x, y, z)

def z_error(x, y, z, a, b, d):
    return math.sqrt((((a*x + b*y + d) - z)**2).sum())

def perp_error(x, y, z, a, b, c, d):
    length_squared = a**2 + b**2 + c**2
    return ((a * x + b * y + c * z + d) ** 2 / length_squared).sum() 

def report(kind, params):
    a, b, c, d = params
    paramstr = ','.join(['{:.2f}'.format(p) for p in params])
    print('{:7}: params: ({}), z_error: {:>5.2f}, perp_error: {:>5.2f}'.format(
        kind, paramstr, z_error(x, y, z, a, b, d), perp_error(x, y, z, a, b, c, d)))

report('vert', vert_params)
report('perp', perp_params)
report('guess', initial_guess)

X, Y = np.meshgrid(np.arange(-3.0, 3.0, 0.5), np.arange(-3.0, 3.0, 0.5))
fig = plt.figure()
ax = fig.gca(projection='3d')

def Z(X, Y, params):
    a, b, c, d = params
    return -(a*X + b*Y + d)/c

ax.plot_surface(X, Y, Z(X, Y, initial_guess), rstride=1, cstride=1, alpha=0.3, color='magenta')
ax.plot_surface(X, Y, Z(X, Y, vert_params), rstride=1, cstride=1, alpha=0.3, color='yellow')
ax.plot_surface(X, Y, Z(X, Y, perp_params), rstride=1, cstride=1, alpha=0.3, color='green')
ax.scatter(x, y, z, c='r', s=50)
plt.xlabel('X')
plt.ylabel('Y')
ax.set_zlabel('Z')
ax.axis('equal')
ax.axis('tight')
plt.show()

The code above computes the parameters which minimize vertical distance from the plane and perpendicular distance from the plane. We can then compute the total error:

vert   : params: (0.94,0.52,-1.00,0.10), z_error:  2.63, perp_error:  3.21
perp   : params: (-0.68,-0.39,0.63,-0.06), z_error:  9.50, perp_error:  2.96
guess  : params: (0.28,-0.14,0.95,0.00), z_error:  5.22, perp_error: 52.31

Notice that the vert_params minimize z_error, but perp_params minimize perp_error.

enter image description here

The magenta plane corresponds to the initial_guess, the yellow plane corresponds to the vert_params and the green plane corresponds to the perp_params.

unutbu
  • 842,883
  • 184
  • 1,785
  • 1,677
  • I see, that's why I was getting inconsistent results, I'm using different fitting methods. Thank you for the detailed explanation unutbu! – Gabriel Jan 31 '16 at 20:30
  • I have to wait 48 hs to add a bounty :( – Gabriel Jan 31 '16 at 21:44
  • Bounty is set, in 24hs I'll award it :) – Gabriel Feb 02 '16 at 21:51
  • Can you explain how the `model` function works? When I write a [naive implementation](https://gist.github.com/chronodm/f8f8af5d2764b2a89375d5bde22c0839) that calculates the perpendicular distance for each point and sums them up, I get very different results. – David Moles Jan 13 '19 at 19:26
  • 1
    @DavidMoles: Thank you very much for pointing out the discrepancy. There was a square root taken of the `length` that should not have been there. `model` is computing the sum of the *squares* of the perpendicular distance from the points to the plane. Since `np.linalg.lstsq` finds the parameters which minimize the sum of the squares of the vertical z-distances, `model` should also be a sum of squares. [Here](https://ideone.com/7hwUSp) is a modified version of your code showing that the corrected `model` is consistent with your method. – unutbu Jan 14 '19 at 00:17
  • @unutbu Aha! Thanks. – David Moles Jan 14 '19 at 04:15
1

The answer by unutbu is outstanding, the best I've seen online. I've tested it a lot.

To refine it a bit, for making an estimate of the initial guess for a plane, I found it useful to do so using just 3 points from the larger set. We can estimate the parameters a,b,c,d like so:

    def estimate_initial_parameters_with_first_three_points(points):
        # points is a list of lists [[x,y,z], [x,y,z], ...]
        p1 = points[0] 
        p2 = points[1]
        p3 = points[2]
        a1 = p2[0] - p1[0] 
        b1 = p2[1] - p1[1]
        c1 = p2[2] - p1[2]
        a2 = p3[0] - p1[0]
        b2 = p3[1] - p1[1] 
        c2 = p3[2] - p1[2]
        a = b1 * c2 - b2 * c1 
        b = a2 * c1 - a1 * c2 
        c = a1 * b2 - b1 * a2 
        d = (- a * p1[0] - b * p1[1] - c * p1[2]) 
        return [a,b,c,d]

This is of course derived from the equation of a plane:

a(x-x0) + b*(y-y0) + c*(z-z0) = d
legel
  • 2,507
  • 3
  • 23
  • 22