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