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)
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.