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)
CodePudding user response：
I don't think you need to precisely determine center of coordinate system of camera 1
Use camera 1 to get the coordinates for camera 2 at any arbitrary point. Move the camera linearly over a known distance and get the coordinates again. Once you have two coordinates you can use coordinate transformation matrix in section 2.9 in the following document
http://motion.cs.illinois.edu/RoboticSystems/CoordinateTransformations.html
CodePudding user response：
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.