def bodyEstimate(self, x, y, dist, USE_WEBOTS_ESTIMATE=True):
"""
Body estimate takes a pixel on the screen, and a vision calculated
distance to that pixel, and calculates where that pixel is relative
to the world frame. It then returns an estimate to that position,
with units in cm. See also pixEstimate
"""
if dist <= 0.0:
return NULL_ESTIMATE
# all angle signs are according to right hand rule for the major axis
# get bearing angle in image plane,left pos, right negative
object_bearing = (IMAGE_CENTER_X - float(x)) * PIX_TO_RAD_X
# get elevation angle in image plane, up negative, down is postive
object_elevation = (float(y) - IMAGE_CENTER_Y) * PIX_TO_RAD_Y
# convert dist estimate to mm
object_dist = dist * 10
# object in the camera frame
objectInCameraFrame = vector4D(
object_dist * cos(object_bearing) * cos(-object_elevation),
object_dist * sin(object_bearing),
object_dist * cos(object_bearing) * sin(-object_elevation),
)
# object in world frame
objectInWorldFrame = dot(cameraToWorldFrame, objectInCameraFrame)
objectInBodyFrame = dot(cameraToBodyTransform, objectInCameraFrame)
if USE_WEBOTS_ESTIMATE:
badBearing = self.getEstimate(objectInWorldFrame)
goodBearing = self.getEstimate(objectInBodyFrame)
# cout << goodBearing[EST_BEARING] << "\t" << badBearing[EST_BEARING] << endl;
goodEst = Estimate(
[
badBearing[EST_DIST],
goodBearing[EST_ELEVATION],
goodBearing[EST_BEARING],
badBearing[EST_X],
badBearing[EST_Y],
]
)
return goodEst
else:
return self.getEstimate(objectInWorldFrame)
def testPixEstimateStraightForward(self, x, y, camera_pitch=0.0):
""" Reset our orientation to a fixed way - have the camera
rotated along the y axis (pitch) with angle camera_pitch compared to level - we
add the CAMERA_PITCH_ANGLE ourselves.
Test that target_location_WF is recreated correctly.
Notice: when camera looks straight ahead, ditance is x, y becomes the x axis,
and z (height) becomes the x axis.
Return estimate and location of x, y in pixels (which is computed to be fed to the pixEstimate)
"""
camera_pitch = -CAMERA_PITCH_ANGLE + camera_pitch # positive is looking down
ang = [0.0 for i in xrange(26)]
ang[1] = camera_pitch # here positive is looking down
# todo - assert self.cameraToWorldFrame == identity()
self.transform(ang, [0.0, 0.0])
foc_x, foc_y, foc_z = self.focalPointInWorldFrame # we aren't testing this..
# first get values in WF but with Camera origin - this is almost the values we gave, but
# we give values compared to body, so we need to fix them a little
z = bottom_cemera_height_when_level = (foc_z + 331) * MM_TO_CM
x = x - self.focalPointInWorldFrame[X] * MM_TO_CM # compensate for distance camera is looking forward
# now rotate by pitch - doesn't affect y at all, just x, and possibly what's in the frame
pitch = -camera_pitch - CAMERA_PITCH_ANGLE # in this calculation positive pitch is looking up
cp, sp = cos(pitch), sin(pitch)
x_tag = x * cp - z * sp
z_tag = x * sp + z * cp
if pitch < 0:
assert z_tag < z
y = -y # keep right handed coordinate system. x is forward, y is to the left.
pix_x, pix_y = (
float(y) / x_tag * FOCAL_LENGTH_MM / PIX_X_TO_MM + IMAGE_CENTER_X,
float(z_tag) / x_tag * FOCAL_LENGTH_MM / PIX_Y_TO_MM + IMAGE_CENTER_Y,
)
est = self.pixEstimate(pix_x, pix_y, 0.0)
return est, pix_x, pix_y
