def transform_frame(vec, orient, att, frame_id):
vec = numpy.array(vec)
if frame_id == 'aruco_map':
# from 'aruco_map' to 'main_camera'
rmat, j = Rodrigues(orient)
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
elif frame_id == 'main_camera':
pass
# from 'main_camera' to 'fcu'
a = camera_angle
vec[0], vec[1], vec[2] = vec[1] * math.cos(a) + vec[2] * math.sin(
a), -vec[0], vec[2] * math.cos(a) - vec[1] * math.sin(a)
# from 'fcu' to 'fcu_horiz'
roll, pitch = att[0], att[1]
rmat = euler_matrix(roll, pitch, 0, 'rxyz')
rmat = rmat[0:3, 0:3]
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
return list(vec)
def direct(fiducialPoints, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xyz = rVec.dot(fiducialPoints[0].T) + tVec
xp = xyz[0] / xyz[2]
yp = xyz[1] / xyz[2]
rp2 = xp**2 + yp**2
aux = (distCoeffs[0] + distCoeffs[1]) / (1 + distCoeffs[0] * sqrt(1 + rp2))
xpp = xp * aux
ypp = yp * aux
u = xpp + linearCoeffs[0]
v = ypp + linearCoeffs[1]
return array([u, v]).reshape((fiducialPoints.shape[1], 1, 2))
def direct(fiducialPoints, rVec, tVec, linearCoeffs, distCoeffs):
# format as matrix
if rVec.shape != (3, 3):
rVec, _ = Rodrigues(rVec)
xyz = rVec.dot(fiducialPoints[0].T) + tVec
xp = xyz[0] / xyz[2]
yp = xyz[1] / xyz[2]
rp = sqrt(xp**2 + yp**2)
thetap = arctan(rp)
rpp = distCoeffs * tan(thetap / 2)
rpp_rp = rpp / rp
xpp = xp * rpp_rp
ypp = yp * rpp_rp
u = xpp + linearCoeffs[0]
v = ypp + linearCoeffs[1]
return array([u, v]).reshape((fiducialPoints.shape[1], 1, 2))