horn_trajectory_fit.py

import numpy as np
import matplotlib.pyplot as plt


trajectory_gt = np.array([[-0.3867186903953552, -0.3169364035129547, 0.0],[-0.23194852471351624, -0.1902160346508026, 0.0],[-0.07601896673440933, -0.06425309926271439, 0.0],[0.08405143022537231, 0.05900496616959572, 0.0],[0.2532316744327545, 0.17631226778030396, 0.0],[0.43847835063934326, 0.28312453627586365, 0.0],[0.6483954787254333, 0.3736981153488159, 0.0],[0.889494001865387, 0.44217759370803833, 0.0],[1.1624500751495361, 0.4836835265159607, 0.0],[1.4617658853530884, 0.49441155791282654, 0.0],[1.775768756866455, 0.47163209319114685, 0.0],[2.0870449542999268, 0.41373151540756226, 0.0],[2.3772077560424805, 0.3206641376018524, 0.0],[2.631664514541626, 0.194403275847435, 0.0],[2.8400490283966064, 0.03898302838206291, 0.0],[2.9962236881256104, -0.13950233161449432, 0.0],[3.0981533527374268, -0.3330237865447998, 0.0],[3.1465418338775635, -0.533002495765686, 0.0],[3.143460988998413, -0.7316943407058716, 0.0],[3.092233180999756, -0.9223166108131409, 0.0],[2.9974257946014404, -1.0990463495254517, 0.0],[2.864729642868042, -1.2570346593856812, 0.0],[2.6995959281921387, -1.3925431966781616, 0.0],[2.505875825881958, -1.5030819177627563, 0.0],[2.285691261291504, -1.5874221324920654, 0.0],[2.0394413471221924, -1.6455957889556885, 0.0],[1.766087293624878, -1.6788723468780518, 0.0],[1.466331958770752, -1.689498782157898, 0.0],[1.1457988023757935, -1.6804404258728027, 0.0],[0.815320611000061, -1.6553577184677124, 0.0],[0.4909396469593048, -1.6186047792434692, 0.0],[0.19334745407104492, -1.57513427734375, 0.0],[-0.05827604979276657, -1.5294402837753296, 0.0],[-0.2524245083332062, -1.4844986200332642, 0.0],[-0.3858250677585602, -1.4416722059249878, 0.0],[-0.46343839168548584, -1.4007093906402588, 0.0],[-0.4979982376098633, -1.3598612546920776, 0.0],[-0.5049448609352112, -1.317151665687561, 0.0],[-0.497358113527298, -1.2716481685638428, 0.0],[-0.4854966104030609, -1.223577618598938, 0.0],[-0.4767979681491852, -1.1743268966674805, 0.0],[-0.4759747385978699, -1.1264060735702515, 0.0],[-0.4860725402832031, -1.083060383796692, 0.0],[-0.5095278024673462, -1.0478806495666504, 0.0],[-0.5482640862464905, -1.0247671604156494, 0.0],[-0.6036919355392456, -1.017930030822754, 0.0],[-0.676642894744873, -1.0317085981369019, 0.0],[-0.7666435241699219, -1.0685843229293823, 0.0],[-0.8711889982223511, -1.1271913051605225, 0.0],[-0.9856768250465393, -1.2021371126174927, 0.0],[-1.1034071445465088, -1.2840017080307007, 0.0],[-1.2157480716705322, -1.359693169593811, 0.0],[-1.3139539957046509, -1.4163564443588257, 0.0],[-1.3909844160079956, -1.445281744003296, 0.0],[-1.44166898727417, -1.4422595500946045, 0.0],[-1.4627083539962769, -1.407582402229309, 0.0],[-1.4526530504226685, -1.3458126783370972, 0.0],[-1.411688208580017, -1.2632516622543335, 0.0],[-1.3414161205291748, -1.1654077768325806, 0.0],[-1.2448360919952393, -1.0567655563354492, 0.0],[-1.126344919204712, -0.9407861232757568, 0.0],[-0.9916239976882935, -0.8199328780174255, 0.0],[-0.8464055061340332, -0.695966362953186, 0.0],[-0.6952348351478577, -0.570234477519989, 0.0],[-0.541358470916748, -0.4437001645565033, 0.0]])

angle = 0.1
R_gt =  np.array([[np.cos(angle),-np.sin(angle),0],[np.sin(angle),np.cos(angle),0],[0,0,1]])
T_gt = [0,0.2,0]

def tranformTrajectory(trajectory, R,t):
    l = trajectory.shape[0]
    trajectory_l =  np.zeros((l,3))
    for i in range(l):
        p = trajectory[i]
        trajectory_l[i] = R@p + t
    return trajectory_l

def simulateGPS(trajectory, n):
    gps_points = np.zeros((n,3))
    trajectory_points =  np.zeros((n,3))
    l = trajectory.shape[0]
    for i in range(n):
        r = np.random.randint(0,l)
        p = trajectory[r]
        gps_points[i] = (p+0.01*np.random.randn(1,3))
        trajectory_points[i] = p
    return(np.array(trajectory_points),np.array(gps_points))

def horn(P, Q):
    
    if P.shape != Q.shape:
        print("Matrices P and Q must be of the same dimensionality")
        sys.exit(1)

    centroids_P = np.mean(P, axis=1)
    centroids_Q = np.mean(Q, axis=1)
    A = P - np.outer(centroids_P, np.ones(P.shape[1]))
    B = Q - np.outer(centroids_Q, np.ones(Q.shape[1]))
    C = np.dot(A, B.transpose())
    U, S, V = np.linalg.svd(C)
    R = np.dot(V.transpose(), U.transpose())
    L = np.eye(3)
    if(np.linalg.det(R) < 0):
        L = np.eye(3)
        L[2][2] *= -1

    R = np.dot(V.transpose(), np.dot(L, U.transpose()))
    t = np.dot(-R, centroids_P) + centroids_Q
    return (R, t)


if __name__=="__main__":
    
    P,Q = simulateGPS(trajectory_gt, 15)
    P = tranformTrajectory(P, R_gt, T_gt)
    trajectory =  tranformTrajectory(trajectory_gt, R_gt, T_gt)


    plt.subplot(131)
    plt.title("Input")
    plt.plot(trajectory[:,0],trajectory[:,1])
    plt.scatter (Q[:,0],Q[:,1], c='r')
    plt.subplot(132)
    
    plt.title("Fit with Horn's method")
    # recover using horn
    R,t = horn(P.transpose(),Q.transpose())
    trajectory_fitted = tranformTrajectory(trajectory ,R,t)

    plt.plot(trajectory_fitted[:,0],trajectory_fitted[:,1])
    plt.scatter (Q[:,0],Q[:,1], c='r')

    plt.subplot(133)
    plt.title("Ground truth")
    # recover using horn
    plt.plot(trajectory_gt[:,0],trajectory_gt[:,1])
    plt.scatter (Q[:,0],Q[:,1], c='r')
    plt.show()