asr_mild_calibration_tool: calibration

Go to the documentation of this file.
00001 
00018 #include "calibration_object.h"
00019 #ifndef Q_MOC_RUN
00020 #include <ros/ros.h>
00021 #endif
00022 
00023 Calibration_Object::Calibration_Object()
00024 {
00025     calculateTransformationFrames();
00026 }
00027 
00028 void Calibration_Object::calculateTransformationFrames()
00029 {
00030     //Initialize default values
00031 
00032     Eigen::Affine3d transformationLeft = getDHTransformation(marker_left_rotation_z*M_PI/180.0, marker_left_transformation_z, marker_left_rotation_x*M_PI/180.0, marker_left_transformation_x);
00033     frame_marker_left = transformationLeft.matrix();
00034 
00035     Eigen::Affine3d transformationRight = getDHTransformation(marker_right_rotation_z*M_PI/180.0, marker_right_transformation_z, marker_right_rotation_x*M_PI/180.0, marker_right_transformation_x);
00036     frame_marker_right = transformationRight.matrix();
00037     double degreesToRadiant =M_PI/180.0;
00038     Eigen::Translation<double, 3> translation(0.102046, 0.00162421, 0.101897);
00039     Eigen::Affine3d rotationA(Eigen::AngleAxisd(77.75461941777554*degreesToRadiant, Eigen::Vector3d(0, 0, 1.0)));
00040     Eigen::Affine3d rotationB(Eigen::AngleAxisd(-90.30507946507706*degreesToRadiant, Eigen::Vector3d(0.0, 1.0, 0.0)));
00041     Eigen::Affine3d rotationC(Eigen::AngleAxisd(-77.27385977970101*degreesToRadiant, Eigen::Vector3d(1.0, 0, 0.0)));
00042 
00043     /*Eigen::Translation<double, 3> translation(0.116425, -0.001466, 0.070503);
00044     Eigen::Affine3d rotationA(Eigen::AngleAxisd(-89.456491*tt, Eigen::Vector3d(0, 0, 1.0)));
00045     Eigen::Affine3d rotationB(Eigen::AngleAxisd(90.316866*tt, Eigen::Vector3d(1.0, 0, 0.0)));
00046     Eigen::Affine3d rotationC(Eigen::AngleAxisd(89.785602*tt, Eigen::Vector3d(0, 0, 1.0)));*/
00047     Eigen::Affine3d temp  = translation * rotationA *rotationB * rotationC;
00048     frame_marker_right = frame_marker_left * temp.matrix();
00049 
00050     /*Eigen::Translation<double, 3> translation(0, -0.0998, 0.1065);
00051     Eigen::Affine3d rotationA(Eigen::AngleAxisd(90.802397*tt, Eigen::Vector3d(0, 0, 1.0)));
00052     Eigen::Affine3d rotationB(Eigen::AngleAxisd(91.552294*tt, Eigen::Vector3d(1.0, 0, 0.0)));
00053     Eigen::Affine3d rotationC(Eigen::AngleAxisd(-91.280564 *tt, Eigen::Vector3d(0, 0, 1.0)));
00054     Eigen::Affine3d temp  = translation * rotationA *rotationB * rotationC;*/
00055 }
00056 
00057 Eigen::Affine3d Calibration_Object::getDHTransformation(double rotZ, double transZ, double rotX, double transX)
00058 {
00059     Eigen::Affine3d rotationZ(Eigen::AngleAxisd(rotZ, Eigen::Vector3d(0, 0, 1.0)));
00060     Eigen::Translation<double, 3> translation(transX, 0.0, transZ);
00061     Eigen::Affine3d rotationX(Eigen::AngleAxisd(rotX, Eigen::Vector3d(1.0, 0, 0)));
00062     return rotationZ * translation * rotationX;
00063 }