test_cal_job.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  *   http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include <industrial_extrinsic_cal/calibration_job_definition.h>
00020 #include <industrial_extrinsic_cal/runtime_utils.h>
00021 #include <tf/transform_datatypes.h>
00022 #include <tf/transform_listener.h>
00023 #include <tf_conversions/tf_eigen.h>
00024 #include <tf/transform_broadcaster.h>
00025 #include <ros/ros.h>
00026 #include <ros/package.h>
00027 #include <Eigen/Geometry>
00028 #include <Eigen/Core>
00029 
00030 using industrial_extrinsic_cal::CalibrationJob;
00031 using std::string;
00032 
00033 void print_AAtoH(double &x, double &y, double &z, double &tx, double &ty, double &tz);
00034 void print_AAToHI(double x, double y, double z, double tx, double ty, double tz);
00035 void generateMessages(std::vector<geometry_msgs::Pose> poses);
00036 void pblockToPose(industrial_extrinsic_cal::P_BLOCK optimized_input);
00037 ros::Publisher transform_pub1, transform_pub2, transform_pub3, transform_pub4;
00038 std::vector<geometry_msgs::Pose> pose_msgs;
00039 std::vector<tf::Transform> transforms;
00040 
00041 int main(int argc, char **argv)
00042 {
00043   ros::init(argc, argv, "calibration_node");
00044 
00045   industrial_extrinsic_cal::ROSRuntimeUtils utils;
00046   ros::NodeHandle nh;
00047   ros::NodeHandle priv_nh_("~");
00048 
00049   std::string ros_package_name;
00050   std::string launch_file_name;
00051   priv_nh_.getParam("camera_file", utils.camera_file_);
00052   priv_nh_.getParam("target_file", utils.target_file_);
00053   priv_nh_.getParam("cal_job_file", utils.caljob_file_);
00054   priv_nh_.getParam("store_results_package_name", ros_package_name);
00055   priv_nh_.getParam("store_results_file_name", launch_file_name);
00056 
00057   std::string path = ros::package::getPath("industrial_extrinsic_cal");
00058   std::string file_path=path+"/yaml/";
00059   transform_pub1= nh.advertise<geometry_msgs::PoseStamped>("camera1_pose", 1);
00060   transform_pub2= nh.advertise<geometry_msgs::PoseStamped>("camera2_pose", 1);
00061   transform_pub3= nh.advertise<geometry_msgs::PoseStamped>("target1_pose", 1);
00062   transform_pub4= nh.advertise<geometry_msgs::PoseStamped>("target2_pose", 1);
00063 
00064   ROS_INFO_STREAM("camera file: "<<file_path+utils.camera_file_);
00065   industrial_extrinsic_cal::CalibrationJob Cal_job(file_path+utils.camera_file_, file_path+utils.target_file_, file_path+utils.caljob_file_);
00066   //ROS_INFO_STREAM("hello world ");
00067   if (Cal_job.load())
00068   {
00069     ROS_INFO_STREAM("Calibration job (cal_job, target and camera) yaml parameters loaded.");
00070   }
00071 
00072   utils.world_frame_=Cal_job.getReferenceFrame();
00073   utils.camera_optical_frame_=Cal_job.getCameraOpticalFrame();
00074   utils.camera_intermediate_frame_=Cal_job.getCameraIntermediateFrame();
00075   utils.initial_extrinsics_ = Cal_job.getOriginalExtrinsics();
00076   utils.target_frame_=Cal_job.getTargetFrames();
00077   industrial_extrinsic_cal::P_BLOCK orig_extrinsics;
00078   tf::Transform tf_camera_orig;
00079 
00080   for (int k=0; k<utils.initial_extrinsics_.size(); k++ )
00081   {
00082     //ROS_INFO_STREAM("each camera extrinsics: "<<utils.initial_extrinsics_[k][0]<<" "<<utils.initial_extrinsics_[k][1]<<" "
00083       //            <<utils.initial_extrinsics_[k][2]<<" "<<utils.initial_extrinsics_[k][3]<<" "
00084         //          <<utils.initial_extrinsics_[k][4]<<" "<<utils.initial_extrinsics_[k][5]);
00085     orig_extrinsics=utils.initial_extrinsics_[k];
00086     ROS_INFO_STREAM("Original Camera "<<k);
00087     tf_camera_orig= utils.pblockToPose(orig_extrinsics);
00088     utils.initial_transforms_.push_back(tf_camera_orig);
00089   }
00090   utils.broadcasters_.resize(utils.initial_extrinsics_.size());
00091   ROS_INFO_STREAM("Size of broadcasters: "<<utils.broadcasters_.size());
00092   ROS_INFO_STREAM("Size of initial_transforms: "<<utils.initial_transforms_.size());
00093   ROS_INFO_STREAM("Size of cam_int_frame: "<<utils.camera_intermediate_frame_.size());
00094   ROS_INFO_STREAM("Size of target_frame: "<<utils.target_frame_.size());
00095   ROS_INFO_STREAM("Target frame: "<<utils.target_frame_[0]);
00096   for (int k=0; k<utils.broadcasters_.size(); k++ )
00097   {
00098     utils.broadcasters_[k].sendTransform(tf::StampedTransform(utils.initial_transforms_[k], ros::Time::now(),
00099                                                               utils.target_frame_[0], utils.camera_intermediate_frame_[k]));
00100   }
00101 
00102   if (Cal_job.run())
00103   {
00104     ROS_INFO_STREAM("Calibration job observations and optimization complete");
00105   }
00106 
00107   utils.calibrated_extrinsics_ = Cal_job.getExtrinsics();
00108   utils.target_poses_ = Cal_job.getTargetPose();
00109   ROS_DEBUG_STREAM("Size of optimized_extrinsics_: "<<utils.calibrated_extrinsics_.size());
00110   ROS_DEBUG_STREAM("Size of targets_: "<<utils.target_poses_.size());
00111 
00112   industrial_extrinsic_cal::P_BLOCK optimized_extrinsics, target;
00113   tf::Transform tf_camera, tf_target;
00114   for (int k=0; k<utils.calibrated_extrinsics_.size(); k++ )
00115   {
00116     optimized_extrinsics=utils.calibrated_extrinsics_[k];
00117     /*print_AAtoH(optimized_extrinsics[0], optimized_extrinsics[1], optimized_extrinsics[2],
00118                 optimized_extrinsics[3], optimized_extrinsics[4], optimized_extrinsics[5]);
00119     print_AAToHI(optimized_extrinsics[0], optimized_extrinsics[1], optimized_extrinsics[2],
00120                  optimized_extrinsics[3], optimized_extrinsics[4], optimized_extrinsics[5]);*/
00121     ROS_INFO_STREAM("Optimized Camera "<<k);
00122     tf_camera=utils.pblockToPose(optimized_extrinsics);
00123     utils.calibrated_transforms_.push_back(tf_camera);
00124     //pblockToPose(optimized_extrinsics);
00125   }
00126   tf::Vector3 tf_cal=tf_camera.getOrigin();
00127   ROS_INFO_STREAM("Cal transform origin: "<<tf_cal.x()<<" "<<tf_cal.y()<<" "<<tf_cal.z());
00128   for (int k=0; k<utils.target_poses_.size(); k++ )
00129   {
00130     target=utils.target_poses_[k];
00131     ROS_INFO_STREAM("Optimized Target "<<k);
00132     tf_target=utils.pblockToPose(target);
00133     utils.target_transforms_.push_back(tf_target);
00134   }
00135   //generateMessages(pose_msgs);
00136   tf::StampedTransform temp_tf_cam, temp_tf_world;
00137   for (int i=0; i<utils.calibrated_extrinsics_.size(); i++ )
00138   {
00139     try
00140     {
00141       utils.listener_.waitForTransform(utils.camera_optical_frame_[i],utils.camera_intermediate_frame_[i],
00142                                               ros::Time(0), ros::Duration(3.0));
00143       utils.listener_.lookupTransform( utils.camera_optical_frame_[i],utils.camera_intermediate_frame_[i],
00144                                        ros::Time(0), temp_tf_cam);
00145       utils.camera_internal_transforms_.push_back(temp_tf_cam);
00146     }
00147     catch (tf::TransformException &ex)
00148     {
00149       ROS_ERROR("%s",ex.what());
00150     }
00151   }
00152   ROS_INFO_STREAM("Size of internal_transforms: "<<utils.camera_internal_transforms_.size());
00153   for (int k=0; k<utils.calibrated_transforms_.size(); k++ )
00154   {
00155     utils.calibrated_transforms_[k]=utils.calibrated_transforms_[k]*utils.camera_internal_transforms_[k];
00156   }
00157   ROS_INFO_STREAM("Target frame1: "<<utils.target_frame_[0]);
00158   ROS_INFO_STREAM("World frame: "<<utils.world_frame_);
00159   try
00160   {
00161     utils.listener_.waitForTransform(utils.world_frame_,utils.target_frame_[0], ros::Time(0), ros::Duration(3.0));
00162     utils.listener_.lookupTransform(utils.world_frame_,utils.target_frame_[0], ros::Time(0), temp_tf_world);
00163     utils.points_to_world_transforms_.push_back(temp_tf_world);
00164   }
00165   catch (tf::TransformException &ex)
00166   {
00167     ROS_ERROR("%s",ex.what());
00168   }
00169   for (int k=0; k<utils.calibrated_transforms_.size(); k++ )
00170   {
00171     utils.calibrated_transforms_[k]=utils.points_to_world_transforms_[0]*utils.calibrated_transforms_[k];
00172   }
00173   ROS_INFO_STREAM("Broadcasting transforms...");
00174   for (int k=0; k<utils.broadcasters_.size(); k++ )
00175   {
00176     utils.broadcasters_[k].sendTransform(tf::StampedTransform(utils.calibrated_transforms_[k], ros::Time::now(),
00177                                                               utils.world_frame_, utils.camera_intermediate_frame_[k]));
00178   }
00179   ROS_INFO_STREAM("Camera pose(s) published");
00180   if (Cal_job.store())
00181   {
00182     ROS_INFO_STREAM("Calibration job optimization saved to file");
00183   }
00184 
00185   std::string save_package_path = ros::package::getPath(ros_package_name);
00186   std::string save_file_path = "/launch/"+launch_file_name;
00187   if (utils.store_tf_broadcasters(save_package_path, save_file_path))
00188   {
00189     ROS_INFO_STREAM("Calibration job optimization camera to world transforms saved");
00190   }
00191 
00192 }
00193 
00194 void pblockToPose(industrial_extrinsic_cal::P_BLOCK optimized_input)
00195 {
00196   double R[9];
00197   double aa[3];
00198   aa[0] = optimized_input[0];
00199   aa[1] = optimized_input[1];
00200   aa[2] = optimized_input[2];
00201   ceres::AngleAxisToRotationMatrix(aa, R);
00202   double rx = atan2(R[7], R[8]);
00203   double ry = atan2(-R[6], sqrt(R[7] * R[7] + R[8] * R[8]));
00204   double rz = atan2(R[3], R[0]);
00205   //double rx = atan2(R[5], R[8]);
00206   //double ry = atan2(-R[2], sqrt(R[5] * R[5] + R[8] * R[8]));
00207   //double rz = atan2(R[1], R[0]);
00208   Eigen::Matrix4f mod_matrix;
00209 
00210   double ix = -(optimized_input[3] * R[0] + optimized_input[4] * R[1] + optimized_input[5] * R[2]);
00211   double iy = -(optimized_input[3] * R[3] + optimized_input[4] * R[4] + optimized_input[5] * R[5]);
00212   double iz = -(optimized_input[3] * R[6] + optimized_input[4] * R[7] + optimized_input[5] * R[8]);
00213 
00214   tf::Quaternion tf_quater;
00215   tf::Matrix3x3 tf_mod_matrix;
00216   tf_mod_matrix.setRPY(rx, ry, rz);
00217   tf_mod_matrix.getRotation(tf_quater);
00218   double roll, pitch, yaw;
00219   tf_mod_matrix.getRPY(roll, pitch, yaw);
00220   tf::Vector3 tf_transl;
00221   //tf_transl.setValue(optimized_input[3], optimized_input[4], optimized_input[5]);
00222   tf_transl.setValue(ix, iy, iz);
00223   ROS_INFO_STREAM("Origin: "<< tf_transl.x()<<", " <<tf_transl.y()<<", "<<tf_transl.z());
00224   ROS_INFO_STREAM("Roll, pitch, yaw: "<< roll <<", " <<pitch<<", "<<yaw);
00225   tf::Transform tf_model;
00226   tf_model.setRotation(tf_quater);
00227   tf_model.setOrigin(tf_transl);
00228   transforms.push_back(tf_model);
00229   //tf_models.push_back(tf_model);
00230   geometry_msgs::Pose pose_msg;
00231   tf::poseTFToMsg(tf_model, pose_msg);
00232   pose_msgs.push_back(pose_msg);
00233 }
00234 
00235 void generateMessages(std::vector<geometry_msgs::Pose> poses)
00236 {
00237 
00238   geometry_msgs::PoseStamped pose1_msg, pose2_msg, pose3_msg, pose4_msg;
00239   pose1_msg.pose.orientation = poses[0].orientation;
00240   pose1_msg.pose.position =poses[0].position;
00241   pose1_msg.header.frame_id = "/world_frame";
00242   pose1_msg.header.stamp=ros::Time::now();
00243 
00244   pose2_msg.pose.orientation = poses[1].orientation;
00245   pose2_msg.pose.position = poses[1].position;
00246   pose2_msg.header.frame_id = "/world_frame";
00247   pose2_msg.header.stamp=ros::Time::now();
00248   transform_pub1.publish(pose1_msg);
00249   transform_pub2.publish(pose2_msg);
00250 
00251   pose3_msg.pose.orientation = poses[2].orientation;
00252   pose3_msg.pose.position = poses[2].position;
00253   pose3_msg.header.frame_id = "/world_frame";
00254   pose3_msg.header.stamp=ros::Time::now();
00255 
00256   pose4_msg.pose.orientation =poses[3].orientation;
00257   pose4_msg.pose.position = poses[3].position;
00258   pose4_msg.header.frame_id = "/world_frame";
00259   pose4_msg.header.stamp=ros::Time::now();
00260   transform_pub3.publish(pose3_msg);
00261   transform_pub4.publish(pose4_msg);
00262 }
00263 // angle axis to homogeneous transform
00264 void print_AAtoH(double &x, double &y, double &z, double &tx, double &ty, double &tz)
00265 {
00266   double R[9];
00267   double aa[3];
00268   aa[0] = x;
00269   aa[1] = y;
00270   aa[2] = z;
00271   ceres::AngleAxisToRotationMatrix(aa, R);
00272   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[3], R[6], tx);
00273   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[1], R[4], R[7], ty);
00274   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[2], R[5], R[8], tz);
00275   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
00276 }
00277 // angle axis to homogeneous transform inverted
00278 void print_AAToHI(double x, double y, double z, double tx, double ty, double tz)
00279 {
00280   double R[9];
00281   double aa[3];
00282   aa[0] = x;
00283   aa[1] = y;
00284   aa[2] = z;
00285   ceres::AngleAxisToRotationMatrix(aa, R);
00286   double ix = -(tx * R[0] + ty * R[1] + tz * R[2]);
00287   double iy = -(tx * R[3] + ty * R[4] + tz * R[5]);
00288   double iz = -(tx * R[6] + ty * R[7] + tz * R[8]);
00289   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[0], R[1], R[2], ix);
00290   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[3], R[4], R[5], iy);
00291   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", R[6], R[7], R[8], iz);
00292   printf("%6.3lf %6.3lf %6.3lf %6.3lf\n", 0.0, 0.0, 0.0, 1.0);
00293 }


industrial_extrinsic_cal
Author(s): Chris Lewis
autogenerated on Wed Aug 26 2015 12:00:27