Zeug

RobotCoordinate * Transformations::cameraCoordinatesInRobotCoordinates(int camera, int xImagePos, int yImagePos, float anglePan, float angleTilt) {
	
	float angleToPosition;
	float distanceToPosition;
	
	float focalLengthVertical, focalLengthHorizontal;
	float panAngleFromCameraToTarget, tiltAngleFromCameraToTarget;
	float cameraPosX, cameraPosY, cameraPosZ;
	
	if(camera == CAMERA_LEFT) {
		
		focalLengthVertical = CAMERA_LEFT_FOCAL_LENGTH_VERTICAL;
		focalLengthHorizontal = CAMERA_LEFT_FOCAL_LENGTH_HORIZONTAL;
		
		panAngleFromCameraToTarget = -atan( (xImagePos - (CAMERA_LEFT_PIXEL_WIDTH/2.0)) / focalLengthHorizontal );
		tiltAngleFromCameraToTarget = atan( (yImagePos - (CAMERA_LEFT_PIXEL_HEIGHT/2.0)) / focalLengthVertical );
		
		cameraPosX = CAMERA_LEFT_POS_X_FROM_NULLPOINT;
		cameraPosY = CAMERA_LEFT_POS_Y_FROM_NULLPOINT;
		cameraPosZ = CAMERA_LEFT_POS_Z_FROM_NULLPOINT;
	}
	else if(camera == CAMERA_MIDDLE) {
		
		focalLengthVertical = CAMERA_MIDDLE_FOCAL_LENGTH_VERTICAL;
		focalLengthHorizontal = CAMERA_MIDDLE_FOCAL_LENGTH_HORIZONTAL;
		
		panAngleFromCameraToTarget = -atan( (xImagePos - (CAMERA_MIDDLE_PIXEL_WIDTH/2.0)) / focalLengthHorizontal );
		tiltAngleFromCameraToTarget = atan( (yImagePos - (CAMERA_MIDDLE_PIXEL_HEIGHT/2.0)) / focalLengthVertical );
		
		cameraPosX = CAMERA_MIDDLE_POS_X_FROM_NULLPOINT;
		cameraPosY = CAMERA_MIDDLE_POS_Y_FROM_NULLPOINT;
		cameraPosZ = CAMERA_MIDDLE_POS_Z_FROM_NULLPOINT;
	}
	else if	(camera == CAMERA_RIGHT) {
		
		focalLengthVertical = CAMERA_RIGHT_FOCAL_LENGTH_VERTICAL;
		focalLengthHorizontal = CAMERA_RIGHT_FOCAL_LENGTH_HORIZONTAL;
		
		panAngleFromCameraToTarget = -atan( (xImagePos - (CAMERA_RIGHT_PIXEL_WIDTH/2.0)) / focalLengthHorizontal );
		tiltAngleFromCameraToTarget = atan( (yImagePos - (CAMERA_RIGHT_PIXEL_HEIGHT/2.0)) / focalLengthVertical );
		
		
		cameraPosX = CAMERA_RIGHT_POS_X_FROM_NULLPOINT;
		cameraPosY = CAMERA_RIGHT_POS_Y_FROM_NULLPOINT;
		cameraPosZ = CAMERA_RIGHT_POS_Z_FROM_NULLPOINT;
	}
	else {
		cerr << "Transformation Camera not specified " << camera;
		return NULL;	
	}
	
	// compute angles in radiant
	float anglePanToTarget = panAngleFromCameraToTarget + anglePan/ 180.0f * M_PI;
	float angleTiltToTarget = M_PI / 2.0f + tiltAngleFromCameraToTarget + angleTilt/ 180.0f * M_PI;
	
	std::cout << "PAN " << panAngleFromCameraToTarget * 180.0f / M_PI << " TILT " << tiltAngleFromCameraToTarget *180.0f / M_PI<< "\n";
	
	RobotCoordinate * position = new RobotCoordinate;
	// move back in degree
	position->angleToPosition = anglePanToTarget * 180.0f / M_PI; 
	position->distanceToPosition = tan(angleTiltToTarget) * cameraPosZ;
	
	position->posX = sin(anglePanToTarget) * position->distanceToPosition + cameraPosX;
	position->posY = cos(anglePanToTarget) * position->distanceToPosition + cameraPosY;
	position->posZ = 0.0;
		
	return position;
}