pythonopencvcamera-calibrationstereo-3ddlt

How can i find camera location in a stereo camera setup in python?


I tring to reconstruct a scene using a stereo setup and opencv. I managed to calibrate the setup and the DLT seens to work fine. I want to plot the camera position, but i having troubles. I use to think that the origin of the coord system is the image center of camera 1, but the point (0,0,0) does not give a reasonable location for the camera 1. I have tried to get the camera 2 position by np.dot(R.T,T), but whitout camera 1 positions it's hard to evaluate if it is reasonable. If someone could give me insights, it would be great.

import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d as a3
# Intrinsics
mtx1 = np.array([[936.17665765,   0.        , 338.93972505],
                 [  0.        , 938.18174925, 255.1324264 ],
                 [  0.        ,   0.        ,   1.        ]])
mtx2 = np.array([[960.84540587,   0.        , 332.61757678],
                 [  0.        , 958.87725542, 268.9326861 ],
                 [  0.        ,   0.        ,   1.        ]])
# Extrinsics
R = np.array ([[ 0.97041507, -0.16493577, -0.17632579],
               [ 0.2412119 ,  0.69420849,  0.67815292],
               [ 0.01055518, -0.70062169,  0.71345485]])
T = np.array([[  48.72757104],
              [-176.05034858],
              [  79.63471749]])
# get the already DLTized points
reference_board_points = np.array ([[-47.180317, -46.18885 , 294.5223  ],
                                    [-51.83506 , -23.568539, 306.45807 ],
                                    [-65.58375 , -15.979432, 287.46024 ],
                                    [-61.039295, -38.794373, 275.50327 ]])
calibration_board_points = np.array([[[ 3.60635109e+01, -5.00746384e+01,  2.45566589e+02],
                                    [ 3.86718369e+01, -4.85908623e+01,  2.44162781e+02],
                                    [ 4.13089066e+01, -4.72156410e+01,  2.42848755e+02],
                                    [ 4.39814758e+01, -4.57752914e+01,  2.41522430e+02],
                                    [ 4.66319771e+01, -4.43774605e+01,  2.40132523e+02],
                                    [ 4.93025932e+01, -4.29653130e+01,  2.38822464e+02],
                                    [ 5.19983368e+01, -4.15875092e+01,  2.37485733e+02],
                                    [ 3.65380630e+01, -4.82754898e+01,  2.48291428e+02],
                                    [ 3.91755676e+01, -4.68376923e+01,  2.46919998e+02],
                                    [ 4.18002777e+01, -4.54289398e+01,  2.45601593e+02],
                                    [ 4.44731903e+01, -4.40011444e+01,  2.44249420e+02],
                                    [ 4.71368752e+01, -4.25989914e+01,  2.42899841e+02],
                                    [ 4.98036270e+01, -4.11972313e+01,  2.41559433e+02],
                                    [ 5.24993553e+01, -3.98008232e+01,  2.40267975e+02],
                                    [ 3.70277443e+01, -4.65063362e+01,  2.51023300e+02],
                                    [ 3.96574974e+01, -4.50605736e+01,  2.49675385e+02],
                                    [ 4.22981720e+01, -4.36732254e+01,  2.48361145e+02],
                                    [ 4.49654083e+01, -4.22176285e+01,  2.47003220e+02],
                                    [ 4.76237221e+01, -4.08189468e+01,  2.45639954e+02],
                                    [ 5.03158379e+01, -3.94513702e+01,  2.44361755e+02],
                                    [ 5.29908371e+01, -3.80345879e+01,  2.42982971e+02],
                                    [ 3.75172043e+01, -4.46805611e+01,  2.53714920e+02],
                                    [ 4.01533699e+01, -4.32985573e+01,  2.52400894e+02],
                                    [ 4.28166542e+01, -4.18831062e+01,  2.51108307e+02],
                                    [ 4.54714279e+01, -4.04463692e+01,  2.49730392e+02],
                                    [ 4.81342926e+01, -3.90643463e+01,  2.48431137e+02],
                                    [ 5.08134079e+01, -3.76832466e+01,  2.47096497e+02],
                                    [ 5.34982262e+01, -3.62685547e+01,  2.45791138e+02],
                                    [ 3.80413094e+01, -4.29372025e+01,  2.56491486e+02],
                                    [ 4.06504517e+01, -4.14789162e+01,  2.55135910e+02],
                                    [ 4.33081589e+01, -4.01352997e+01,  2.53844345e+02],
                                    [ 4.59516678e+01, -3.86624756e+01,  2.52464874e+02],
                                    [ 4.86417274e+01, -3.73473511e+01,  2.51214813e+02],
                                    [ 5.13202438e+01, -3.58843803e+01,  2.49833557e+02],
                                    [ 5.40074043e+01, -3.45536232e+01,  2.48554916e+02],
                                    [ 3.85139580e+01, -4.11190376e+01,  2.59170258e+02],
                                    [ 4.11453819e+01, -3.97201157e+01,  2.57792725e+02],
                                    [ 4.37994576e+01, -3.82616539e+01,  2.56518250e+02],
                                    [ 4.64612846e+01, -3.69307442e+01,  2.55167526e+02],
                                    [ 4.91439590e+01, -3.55131226e+01,  2.53914734e+02],
                                    [ 5.18221283e+01, -3.41468468e+01,  2.52576965e+02],
                                    [ 5.45313225e+01, -3.27379837e+01,  2.51290359e+02],
                                    [ 3.90157280e+01, -3.94710922e+01,  2.61982605e+02],
                                    [ 4.16454887e+01, -3.79300385e+01,  2.60563995e+02],
                                    [ 4.43150024e+01, -3.66537628e+01,  2.59310242e+02],
                                    [ 4.69526482e+01, -3.51042480e+01,  2.57902405e+02],
                                    [ 4.96437798e+01, -3.38314095e+01,  2.56660187e+02],
                                    [ 5.23159561e+01, -3.23550148e+01,  2.55297729e+02],
                                    [ 5.50447426e+01, -3.10470638e+01,  2.54027512e+02]],

                                   [[-2.18739090e+01, -9.47645092e+00,  2.47313171e+02],
                                    [-1.88172512e+01, -9.10081387e+00,  2.48462280e+02],
                                    [-1.57593975e+01, -8.69764614e+00,  2.49631348e+02],
                                    [-1.27493162e+01, -8.31858540e+00,  2.50814362e+02],
                                    [-9.68059349e+00, -7.94558477e+00,  2.51984802e+02],
                                    [-6.65390301e+00, -7.56426382e+00,  2.53189728e+02],
                                    [-3.61760330e+00, -7.23047543e+00,  2.54423935e+02],
                                    [-2.27319984e+01, -6.55116940e+00,  2.48588379e+02],
                                    [-1.96747971e+01, -6.16053438e+00,  2.49772507e+02],
                                    [-1.66329651e+01, -5.76371861e+00,  2.50939377e+02],
                                    [-1.35759287e+01, -5.36990690e+00,  2.52121094e+02],
                                    [-1.05261564e+01, -5.01409769e+00,  2.53316422e+02],
                                    [-7.51462984e+00, -4.65775633e+00,  2.54542358e+02],
                                    [-4.48242044e+00, -4.28843451e+00,  2.55733261e+02],
                                    [-2.35882263e+01, -3.57698870e+00,  2.49884598e+02],
                                    [-2.05375595e+01, -3.17783117e+00,  2.51035263e+02],
                                    [-1.74898300e+01, -2.81708002e+00,  2.52229828e+02],
                                    [-1.44311256e+01, -2.46626639e+00,  2.53435013e+02],
                                    [-1.13961210e+01, -2.05564213e+00,  2.54606430e+02],
                                    [-8.35307121e+00, -1.70406377e+00,  2.55820557e+02],
                                    [-5.34799147e+00, -1.33975792e+00,  2.57039978e+02],
                                    [-2.44446526e+01, -6.32025719e-01,  2.51175613e+02],
                                    [-2.13928108e+01, -2.14357853e-01,  2.52336914e+02],
                                    [-1.83335438e+01,  1.34691402e-01,  2.53509903e+02],
                                    [-1.52856998e+01,  5.44854999e-01,  2.54698273e+02],
                                    [-1.22412415e+01,  8.90990973e-01,  2.55904297e+02],
                                    [-9.21655464e+00,  1.25824142e+00,  2.57089233e+02],
                                    [-6.17280865e+00,  1.61059284e+00,  2.58332611e+02],
                                    [-2.52852001e+01,  2.35214043e+00,  2.52470779e+02],
                                    [-2.22415180e+01,  2.76856756e+00,  2.53554871e+02],
                                    [-1.91766129e+01,  3.11673880e+00,  2.54760315e+02],
                                    [-1.61291389e+01,  3.48542452e+00,  2.55974274e+02],
                                    [-1.30857773e+01,  3.87846375e+00,  2.57159637e+02],
                                    [-1.00533829e+01,  4.23038864e+00,  2.58358307e+02],
                                    [-7.02779150e+00,  4.56901121e+00,  2.59632446e+02],
                                    [-2.61311569e+01,  5.33716440e+00,  2.53694672e+02],
                                    [-2.30751476e+01,  5.74360418e+00,  2.54835770e+02],
                                    [-2.00044937e+01,  6.11351347e+00,  2.55994049e+02],
                                    [-1.69604950e+01,  6.48697186e+00,  2.57189240e+02],
                                    [-1.39071932e+01,  6.83321476e+00,  2.58443054e+02],
                                    [-1.08897314e+01,  7.22340250e+00,  2.59584869e+02],
                                    [-7.86594629e+00,  7.54717493e+00,  2.60883636e+02],
                                    [-2.69749241e+01,  8.32992554e+00,  2.54944214e+02],
                                    [-2.39082317e+01,  8.73782349e+00,  2.56063324e+02],
                                    [-2.08521576e+01,  9.12201214e+00,  2.57255432e+02],
                                    [-1.77943230e+01,  9.47646904e+00,  2.58431580e+02],
                                    [-1.47555733e+01,  9.82645798e+00,  2.59684601e+02],
                                    [-1.17340612e+01,  1.01691732e+01,  2.60893982e+02],
                                    [-8.72443104e+00,  1.05237551e+01,  2.62139557e+02]],

                                   [[ 4.43082123e+01,  1.06936016e+01,  2.29823868e+02],
                                    [ 4.76057205e+01,  1.13034735e+01,  2.29966049e+02],
                                    [ 5.09290428e+01,  1.18601532e+01,  2.30177521e+02],
                                    [ 5.42619247e+01,  1.24365950e+01,  2.30388428e+02],
                                    [ 5.75769157e+01,  1.29895315e+01,  2.30605682e+02],
                                    [ 6.09234657e+01,  1.35690699e+01,  2.30835938e+02],
                                    [ 6.42630005e+01,  1.40996609e+01,  2.31117538e+02],
                                    [ 4.36962395e+01,  1.37291069e+01,  2.31232925e+02],
                                    [ 4.70168419e+01,  1.42976761e+01,  2.31389679e+02],
                                    [ 5.03405914e+01,  1.48709316e+01,  2.31614731e+02],
                                    [ 5.36636238e+01,  1.54314327e+01,  2.31802124e+02],
                                    [ 5.70110664e+01,  1.59952497e+01,  2.32051178e+02],
                                    [ 6.03262520e+01,  1.65575809e+01,  2.32262421e+02],
                                    [ 6.36793671e+01,  1.71229649e+01,  2.32541290e+02],
                                    [ 4.31151733e+01,  1.67169876e+01,  2.32630157e+02],
                                    [ 4.64306946e+01,  1.73183594e+01,  2.32811050e+02],
                                    [ 4.97562294e+01,  1.78843956e+01,  2.33002701e+02],
                                    [ 5.30915565e+01,  1.84607258e+01,  2.33229828e+02],
                                    [ 5.64241447e+01,  1.90122261e+01,  2.33478882e+02],
                                    [ 5.97272606e+01,  1.95811920e+01,  2.33688766e+02],
                                    [ 6.30687637e+01,  2.01191616e+01,  2.33964111e+02],
                                    [ 4.25363464e+01,  1.97683849e+01,  2.34028168e+02],
                                    [ 4.58488121e+01,  2.03180790e+01,  2.34201401e+02],
                                    [ 4.91808434e+01,  2.08923588e+01,  2.34453598e+02],
                                    [ 5.24921684e+01,  2.14670410e+01,  2.34630737e+02],
                                    [ 5.58321991e+01,  2.20401325e+01,  2.34887650e+02],
                                    [ 5.91418686e+01,  2.25956993e+01,  2.35086777e+02],
                                    [ 6.24824829e+01,  2.31529408e+01,  2.35353134e+02],
                                    [ 4.19646416e+01,  2.27582607e+01,  2.35415970e+02],
                                    [ 4.52662010e+01,  2.33665791e+01,  2.35567032e+02],
                                    [ 4.85868187e+01,  2.39259758e+01,  2.35799469e+02],
                                    [ 5.19356995e+01,  2.45056324e+01,  2.36054657e+02],
                                    [ 5.52496986e+01,  2.50461712e+01,  2.36264893e+02],
                                    [ 5.85905113e+01,  2.56012955e+01,  2.36531250e+02],
                                    [ 6.19005013e+01,  2.61604404e+01,  2.36786591e+02],
                                    [ 4.13767967e+01,  2.58074932e+01,  2.36772705e+02],
                                    [ 4.46779747e+01,  2.63713207e+01,  2.36972321e+02],
                                    [ 4.80037498e+01,  2.69521770e+01,  2.37186508e+02],
                                    [ 5.13369484e+01,  2.75188160e+01,  2.37421646e+02],
                                    [ 5.46556244e+01,  2.81023483e+01,  2.37646347e+02],
                                    [ 5.79839249e+01,  2.86392136e+01,  2.37883453e+02],
                                    [ 6.13243294e+01,  2.92046738e+01,  2.38174011e+02],
                                    [ 4.07929420e+01,  2.88373680e+01,  2.38104446e+02],
                                    [ 4.41129265e+01,  2.94314384e+01,  2.38337509e+02],
                                    [ 4.74283600e+01,  3.00051937e+01,  2.38543869e+02],
                                    [ 5.07467957e+01,  3.05769939e+01,  2.38760025e+02],
                                    [ 5.40747528e+01,  3.11371918e+01,  2.38994461e+02],
                                    [ 5.73999329e+01,  3.16926632e+01,  2.39303665e+02],
                                    [ 6.07331734e+01,  3.22422295e+01,  2.39566483e+02]],

                                   [[ 3.19999771e+01,  1.52994194e+01,  2.86253876e+02],
                                    [ 3.42086525e+01,  1.64329300e+01,  2.84130737e+02],
                                    [ 3.64526100e+01,  1.75409088e+01,  2.82076660e+02],
                                    [ 3.87041359e+01,  1.86577778e+01,  2.79936951e+02],
                                    [ 4.09163475e+01,  1.97589283e+01,  2.77883972e+02],
                                    [ 4.31707611e+01,  2.09370975e+01,  2.75780029e+02],
                                    [ 4.54174767e+01,  2.20242805e+01,  2.73711700e+02],
                                    [ 3.14038200e+01,  1.84321270e+01,  2.87210754e+02],
                                    [ 3.36071892e+01,  1.95418816e+01,  2.85136902e+02],
                                    [ 3.58615189e+01,  2.06736431e+01,  2.83067230e+02],
                                    [ 3.80758820e+01,  2.17778454e+01,  2.81009644e+02],
                                    [ 4.03060112e+01,  2.29398098e+01,  2.78885223e+02],
                                    [ 4.25252914e+01,  2.40438881e+01,  2.76790588e+02],
                                    [ 4.47755737e+01,  2.52088337e+01,  2.74632141e+02],
                                    [ 3.07803974e+01,  2.15635185e+01,  2.88272675e+02],
                                    [ 3.30052071e+01,  2.26810913e+01,  2.86199280e+02],
                                    [ 3.52371826e+01,  2.37782173e+01,  2.84149780e+02],
                                    [ 3.74700470e+01,  2.49554405e+01,  2.81930084e+02],
                                    [ 3.96973076e+01,  2.60495701e+01,  2.79902893e+02],
                                    [ 4.19313354e+01,  2.72093735e+01,  2.77767883e+02],
                                    [ 4.41594582e+01,  2.83269882e+01,  2.75686432e+02],
                                    [ 3.01996384e+01,  2.47174549e+01,  2.89259308e+02],
                                    [ 3.24210281e+01,  2.58038998e+01,  2.87210785e+02],
                                    [ 3.46588593e+01,  2.69520226e+01,  2.85108826e+02],
                                    [ 3.68626671e+01,  2.80844421e+01,  2.83025513e+02],
                                    [ 3.91135368e+01,  2.92181187e+01,  2.80892456e+02],
                                    [ 4.13537445e+01,  3.03614674e+01,  2.78800110e+02],
                                    [ 4.35861855e+01,  3.15099792e+01,  2.76722321e+02],
                                    [ 2.96242085e+01,  2.78294029e+01,  2.90347412e+02],
                                    [ 3.18308258e+01,  2.89732780e+01,  2.88201721e+02],
                                    [ 3.40817032e+01,  3.00738945e+01,  2.86167786e+02],
                                    [ 3.63107033e+01,  3.12426109e+01,  2.84023865e+02],
                                    [ 3.85441437e+01,  3.23672981e+01,  2.81977264e+02],
                                    [ 4.07835922e+01,  3.35314331e+01,  2.79803131e+02],
                                    [ 4.30050735e+01,  3.46469612e+01,  2.77762909e+02],
                                    [ 2.90506439e+01,  3.09576378e+01,  2.91400055e+02],
                                    [ 3.12650375e+01,  3.20994492e+01,  2.89300018e+02],
                                    [ 3.35043030e+01,  3.32282791e+01,  2.87195831e+02],
                                    [ 3.57383385e+01,  3.43740616e+01,  2.85096558e+02],
                                    [ 3.79893913e+01,  3.55343895e+01,  2.82977051e+02],
                                    [ 4.02104874e+01,  3.66533470e+01,  2.80911530e+02],
                                    [ 4.24779358e+01,  3.78305931e+01,  2.78758057e+02],
                                    [ 2.84850063e+01,  3.41009369e+01,  2.92473114e+02],
                                    [ 3.07331905e+01,  3.52618866e+01,  2.90324493e+02],
                                    [ 3.29715614e+01,  3.63840446e+01,  2.88308563e+02],
                                    [ 3.52040062e+01,  3.75315475e+01,  2.86163544e+02],
                                    [ 3.74417305e+01,  3.86500549e+01,  2.84064545e+02],
                                    [ 3.96930733e+01,  3.98425255e+01,  2.81993622e+02],
                                    [ 4.19260559e+01,  4.09757118e+01,  2.79832275e+02]]])

# instantiate figure and axis
fig = plt.figure(figsize=[10, 6],dpi=100)
ax = a3.Axes3D(fig,auto_add_to_figure=False,azim=120,elev=30)
fig.add_axes(ax)
# plot reference board
ax.scatter3D(xs=reference_board_points[:,0],
             ys=reference_board_points[:,1],
             zs=reference_board_points[:,2])
# plot calibration board per view
for view in range(calibration_board_points.shape[0]):
    ax.scatter3D(xs=calibration_board_points[view][:,0],
                 ys=calibration_board_points[view][:,1],
                 zs=calibration_board_points[view][:,2])
# adjust aspect ratio
max_range=np.max([np.diff(func) for func in [ax.get_xlim(),ax.get_ylim(),ax.get_zlim()]])
ax.set_xlim(np.mean(ax.get_xlim())-max_range/2,np.mean(ax.get_xlim())+max_range/2)
ax.set_ylim(np.mean(ax.get_ylim())-max_range/2,np.mean(ax.get_ylim())+max_range/2)
ax.set_zlim(np.mean(ax.get_zlim())-max_range/2,np.mean(ax.get_zlim())+max_range/2)

Solution

  • In general camera 1 location does not need to be in (0,0,0) but it is usually a convention.

    For each camera, the formula to get the camera location is:

    C = - transpose(R) . t
    

    where R is the rotation matrix of that camera and t is its translation vector. It works even if the first camera is in the origin (because t would be zero).

    Having only one R and T in your data, for sure the camera 1 is in (0,0,0). And the second camera should be in:

    -(R.T).dot(T) = [[ -5.66108934][186.04637852][ 71.16521034]]
    

    N.B Inverting a rotation matrix should have the same effect of transposing, but some numerical differences are present. Some prefer to use np.linalg.inv(R).dot(T).

    Another issue that comes to my mind: you may be wrong of a certain multiplicative factor if during calibration you didn't correctly set the square size in world units.