// transformation of camera focal point to body center
// ORDER MATTERS EXTREMELY. READ AustinVilla04.pdf for more.
point3 <float> Pose::calcFocalPointInBodyFrame() {
/*
for (int chain = 0; chain < NUM_CHAINS; chain++ ){
//get mdh params for this chain
}
*/
//OLD STUFF:
// clears matrix values each time method is called (once per frame)
image_matrix.clear();
image_matrix.rotateY(body_roll_angle); // rotate body roll around y axis
image_matrix.rotateX(body_tilt_angle); // rotate body tilt around x axis
// translate based on offsets between neck joint base and center body (0,0,0)
image_matrix.translate(0,NECK_BASE_TO_CENTER_Y,NECK_BASE_TO_CENTER_Z);
image_matrix.rotateX(neck_angle); // rotate neck angle around x axis
image_matrix.rotateZ(pan_angle); // rotate pan angle around z axis
// translate about neck's actual length on z-axis
image_matrix.translate(0, 0, NECK_LENGTH);
image_matrix.rotateX(yaw_angle); // rotate yaw angle around x axis
// translate about yaw joint to camera focal point offsets in y, z axis
image_matrix.translate(0,CAMERA_YAW_Y_LENGTH,-CAMERA_YAW_Z_LENGTH);
// stores focal point's x,y,z point in the body frame into focal_point_body_frame point3 <float>
point3 <float> focal_point ( image_matrix.getX(), image_matrix.getY(), image_matrix.getZ());
return focal_point;
}
void
pcl::visualization::Camera::computeViewMatrix(Eigen::Matrix4d& view_mat) const
{
//constructs view matrix from camera pos, view up, and the point it is looking at
//this code is based off of gluLookAt http://www.opengl.org/wiki/GluLookAt_code
Eigen::Vector3d focal_point (focal[0], focal[1], focal[2]);
Eigen::Vector3d posv (pos[0] , pos[1] , pos[2]);
Eigen::Vector3d up (view[0] , view[1] , view[2]);
Eigen::Vector3d zAxis = (focal_point - posv).normalized();
Eigen::Vector3d xAxis = zAxis.cross(up).normalized();
// make sure the y-axis is orthogonal to the other two
Eigen::Vector3d yAxis = xAxis.cross (zAxis);
view_mat.block <1, 3> (0, 0) = xAxis;
view_mat.block <1, 3> (1, 0) = yAxis;
view_mat.block <1, 3> (2, 0) = -zAxis;
view_mat.row (3) << 0, 0, 0, 1;
view_mat.block <3, 1> (0, 3) = view_mat.topLeftCorner<3, 3> () * (-posv);
}
