industrial_extrinsic_cal: runtime

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2013, 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/basic_types.h>
00020 #include <industrial_extrinsic_cal/runtime_utils.h>
00021 
00022 namespace industrial_extrinsic_cal
00023 {
00024 
00025 tf::Transform ROSRuntimeUtils::pblockToPose(industrial_extrinsic_cal::P_BLOCK &optimized_input)
00026 {
00027   double R[9];
00028   double aa[3];
00029   aa[0] = optimized_input[0];
00030   aa[1] = optimized_input[1];
00031   aa[2] = optimized_input[2];
00032   ceres::AngleAxisToRotationMatrix(aa, R);
00033   double rx = atan2(R[7], R[8]);
00034   double ry = atan2(-R[6], sqrt(R[7] * R[7] + R[8] * R[8]));
00035   double rz = atan2(R[3], R[0]);
00036   Eigen::Matrix4f mod_matrix;
00037 
00038   double ix = -(optimized_input[3] * R[0] + optimized_input[4] * R[1] + optimized_input[5] * R[2]);
00039   double iy = -(optimized_input[3] * R[3] + optimized_input[4] * R[4] + optimized_input[5] * R[5]);
00040   double iz = -(optimized_input[3] * R[6] + optimized_input[4] * R[7] + optimized_input[5] * R[8]);
00041 
00042   tf::Quaternion tf_quater;
00043   tf::Matrix3x3 tf_mod_matrix;
00044   tf_mod_matrix.setRPY(rx, ry, rz);
00045   tf_mod_matrix.getRotation(tf_quater);
00046   double roll, pitch, yaw;
00047   tf_mod_matrix.getRPY(roll, pitch, yaw);
00048   tf::Vector3 tf_transl;
00049   tf_transl.setValue(ix, iy, iz);
00050   ROS_INFO_STREAM("Origin: "<< tf_transl.x()<<", " <<tf_transl.y()<<", "<<tf_transl.z());
00051   ROS_INFO_STREAM("Roll, pitch, yaw: "<< roll <<", " <<pitch<<", "<<yaw);
00052   tf::Transform transform_output;
00053   transform_output.setRotation(tf_quater);
00054   transform_output.setOrigin(tf_transl);
00055   return transform_output;
00056 }
00057 bool ROSRuntimeUtils::store_tf_broadcasters(std::string &package_path, std::string &file_name)
00058 {
00059   std::string filepath = package_path+file_name;
00060   std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
00061   if (output_file.is_open())
00062   {
00063     ROS_INFO_STREAM("Storing results in: "<<filepath);
00064   }
00065   else
00066   {
00067     ROS_ERROR_STREAM("Unable to open file");
00068     return false;
00069   }
00070   output_file << "<launch>";
00071   //have calibrated transforms
00072   double roll, pitch, yaw, x_position, y_position, z_position;
00073   tf::Vector3 origin;
00074   tf::Matrix3x3 orientation;
00075   for (int i=0; i<calibrated_transforms_.size(); i++)
00076   {
00077     origin = calibrated_transforms_.at(i).getOrigin();
00078     x_position=origin.getX();
00079     y_position=origin.getY();
00080     z_position=origin.getZ();
00081     orientation = calibrated_transforms_.at(i).getBasis();
00082     orientation.getEulerYPR(yaw, pitch, roll);
00083     output_file<<"\n";
00084     output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"world_to_camera"<<i<<"\" args=\"";
00085     //tranform publisher launch files requires x y z yaw pitch roll
00086     output_file<<x_position<< ' '<<y_position<< ' '<<z_position<< ' '<<yaw<< ' '<<pitch<< ' '<<roll ;
00087     output_file<<" "<<world_frame_;
00088     output_file<<" "<<camera_intermediate_frame_[i];
00089     output_file<<" 100\" />";
00090   }
00091   output_file<<"\n";
00092   output_file << "</launch>";
00093   return true;
00094 }
00095 } // end of namespace