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 pixHeightToDistancePlus(self, pixHeight, pix_x, cmHeight, debug=False, verbose=False):
"""
approx. the same as ground plane intersection - actually
should give the same results.
h - object height
f - focal distance in mm
b - object height on the ccd in mm
c - object distance from image center along x axis on ccd in mm
x - distance of object center on ccd along x axis (perpendicular
to the height axis). Note that this is geared for the nao,
which only has pitch and no roll for the camera, assuming the body
frame is parallel to the ground (which should also be correct as
long as the robot isn't falling).
"""
# TODO - do another calculation using the joint poses
# to make sure we are actually parallel to the ground - we
# already have the matrix. (cameraFrameToWorld or something)
# The joint has reversed sign compared to the camera's built in pitch.
pitch = CAMERA_PITCH_ANGLE + self._bodyAngles[HEAD_PITCH_JOINT_INDEX]
observed_height = cmHeight * cos(pitch)
if verbose:
print "pitch deg = %3.3f, cmHeight = %3.3f, observed_height = %3.3f" % (
pitch * 180.0 / pi,
cmHeight,
observed_height,
)
f = FOCAL_LENGTH_MM
c = PIX_X_TO_MM * (pix_x - IMAGE_WIDTH / 2.0)
f_tilde = (c ** 2 + f ** 2) ** 0.5
if verbose:
print "c [mm] = %3.3f, f [mm] = %3.3f, f_tilde = %3.3f" % (c, f, f_tilde)
dist = observed_height * f_tilde / (pixHeight * PIX_Y_TO_MM)
if verbose:
print "dist = %3.3f. no pitch dist = %3.3f, no bearing dist = %3.3f" % (
dist,
dist / cos(pitch),
dist / cos(pitch) / f_tilde * f,
)
if debug:
return dist, dist / cos(pitch), dist / cos(pitch) / f_tilde * f
return dist
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