calibration_job_definition.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 <boost/foreach.hpp>
00021 #include <boost/shared_ptr.hpp>
00022 #include <ros/package.h>
00023 
00024 using std::string;
00025 using boost::shared_ptr;
00026 using boost::make_shared;
00027 using ceres::CostFunction;
00028 
00029 namespace industrial_extrinsic_cal
00030 {
00031 
00032 bool CalibrationJob::load()
00033 {
00034   if(CalibrationJob::loadCamera())
00035   {
00036     ROS_INFO_STREAM("Successfully read in cameras ");
00037   }
00038   else
00039   {
00040     ROS_ERROR_STREAM("Camera file parsing failed");
00041     return false;
00042   }
00043   if(CalibrationJob::loadTarget())
00044   {
00045     ROS_INFO_STREAM("Successfully read in targets");
00046   }
00047   else
00048   {
00049     ROS_ERROR_STREAM("Target file parsing failed");
00050     return false;
00051   }
00052   if(CalibrationJob::loadCalJob())
00053   {
00054     ROS_INFO_STREAM("Successfully read in CalJob");
00055   }
00056   else
00057   {
00058     ROS_ERROR_STREAM("Calibration Job file parsing failed");
00059     return false;
00060   }
00061 
00062   return (true);
00063 } // end load()
00064 
00065 bool CalibrationJob::loadCamera()
00066 {
00067   std::ifstream camera_input_file(camera_def_file_name_.c_str());
00068   if (camera_input_file.fail())
00069     {
00070       ROS_ERROR_STREAM(
00071           "ERROR CalibrationJob::load(), couldn't open camera_input_file:    "
00072           << camera_def_file_name_.c_str());
00073       return (false);
00074     }
00075 
00076   string temp_name, temp_topic, camera_optical_frame, camera_intermediate_frame;
00077   CameraParameters temp_parameters;
00078   P_BLOCK extrinsics;
00079 
00080   unsigned int scene_id;
00081   try
00082   {
00083     YAML::Parser camera_parser(camera_input_file);
00084     YAML::Node camera_doc;
00085     camera_parser.GetNextDocument(camera_doc);
00086 
00087     // read in all static cameras
00088     if (const YAML::Node *camera_parameters = camera_doc.FindValue("static_cameras"))
00089     {
00090       ROS_DEBUG_STREAM("Found "<<camera_parameters->size()<<" static cameras ");
00091       for (unsigned int i = 0; i < camera_parameters->size(); i++)
00092       {
00093         (*camera_parameters)[i]["camera_name"] >> temp_name;
00094         (*camera_parameters)[i]["image_topic"] >> temp_topic;
00095         (*camera_parameters)[i]["camera_optical_frame"] >> camera_optical_frame;
00096         (*camera_parameters)[i]["camera_intermediate_frame"] >> camera_intermediate_frame;
00097         (*camera_parameters)[i]["angle_axis_ax"] >> temp_parameters.angle_axis[0];
00098         (*camera_parameters)[i]["angle_axis_ay"] >> temp_parameters.angle_axis[1];
00099         (*camera_parameters)[i]["angle_axis_az"] >> temp_parameters.angle_axis[2];
00100         (*camera_parameters)[i]["position_x"] >> temp_parameters.position[0];
00101         (*camera_parameters)[i]["position_y"] >> temp_parameters.position[1];
00102         (*camera_parameters)[i]["position_z"] >> temp_parameters.position[2];
00103         (*camera_parameters)[i]["focal_length_x"] >> temp_parameters.focal_length_x;
00104         (*camera_parameters)[i]["focal_length_y"] >> temp_parameters.focal_length_y;
00105         (*camera_parameters)[i]["center_x"] >> temp_parameters.center_x;
00106         (*camera_parameters)[i]["center_y"] >> temp_parameters.center_y;
00107         (*camera_parameters)[i]["distortion_k1"] >> temp_parameters.distortion_k1;
00108         (*camera_parameters)[i]["distortion_k2"] >> temp_parameters.distortion_k2;
00109         (*camera_parameters)[i]["distortion_k3"] >> temp_parameters.distortion_k3;
00110         (*camera_parameters)[i]["distortion_p1"] >> temp_parameters.distortion_p1;
00111         (*camera_parameters)[i]["distortion_p2"] >> temp_parameters.distortion_p2;
00112         // create a static camera
00113         shared_ptr<Camera> temp_camera = make_shared<Camera>(temp_name, temp_parameters, false);
00114         temp_camera->camera_observer_ = make_shared<ROSCameraObserver>(temp_topic);
00115         ceres_blocks_.addStaticCamera(temp_camera);
00116         camera_optical_frames_.push_back(camera_optical_frame);
00117         camera_intermediate_frames_.push_back(camera_intermediate_frame);
00118         extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(temp_name);
00119         original_extrinsics_.push_back(extrinsics);
00120        }
00121     }
00122 
00123     // read in all moving cameras
00124     if (const YAML::Node *camera_parameters = camera_doc.FindValue("moving_cameras"))
00125     {
00126       ROS_DEBUG_STREAM("Found "<<camera_parameters->size() << " moving cameras ");
00127       for (unsigned int i = 0; i < camera_parameters->size(); i++)
00128       {
00129         (*camera_parameters)[i]["camera_name"] >> temp_name;
00130         (*camera_parameters)[i]["image_topic"] >> temp_topic;
00131         (*camera_parameters)[i]["camera_optical_frame"] >> camera_optical_frame;
00132         (*camera_parameters)[i]["camera_intermediate_frame"] >> camera_intermediate_frame;
00133         (*camera_parameters)[i]["angle_axis_ax"] >> temp_parameters.angle_axis[0];
00134         (*camera_parameters)[i]["angle_axis_ay"] >> temp_parameters.angle_axis[1];
00135         (*camera_parameters)[i]["angle_axis_az"] >> temp_parameters.angle_axis[2];
00136         (*camera_parameters)[i]["position_x"] >> temp_parameters.position[0];
00137         (*camera_parameters)[i]["position_y"] >> temp_parameters.position[1];
00138         (*camera_parameters)[i]["position_z"] >> temp_parameters.position[2];
00139         (*camera_parameters)[i]["focal_length_x"] >> temp_parameters.focal_length_x;
00140         (*camera_parameters)[i]["focal_length_y"] >> temp_parameters.focal_length_y;
00141         (*camera_parameters)[i]["center_x"] >> temp_parameters.center_x;
00142         (*camera_parameters)[i]["center_y"] >> temp_parameters.center_y;
00143         (*camera_parameters)[i]["distortion_k1"] >> temp_parameters.distortion_k1;
00144         (*camera_parameters)[i]["distortion_k2"] >> temp_parameters.distortion_k2;
00145         (*camera_parameters)[i]["distortion_k3"] >> temp_parameters.distortion_k3;
00146         (*camera_parameters)[i]["distortion_p1"] >> temp_parameters.distortion_p1;
00147         (*camera_parameters)[i]["distortion_p2"] >> temp_parameters.distortion_p2;
00148         (*camera_parameters)[i]["scene_id"] >> scene_id;
00149         shared_ptr<Camera> temp_camera = make_shared<Camera>(temp_name, temp_parameters, true);
00150         temp_camera->camera_observer_ = make_shared<ROSCameraObserver>(temp_topic);
00151         ceres_blocks_.addMovingCamera(temp_camera, scene_id);
00152         camera_optical_frames_.push_back(camera_optical_frame);
00153         camera_intermediate_frames_.push_back(camera_intermediate_frame);
00154         extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(temp_name);
00155         original_extrinsics_.push_back(extrinsics);
00156       }
00157     }
00158   } // end try
00159   catch (YAML::ParserException& e)
00160   {
00161     ROS_INFO_STREAM("load() Failed to read in moving cameras from  yaml file ");
00162     /*ROS_INFO_STREAM("camera name =     "<<temp_name.c_str());
00163     ROS_INFO_STREAM("angle_axis_ax =  "<<temp_parameters.angle_axis[0]);
00164     ROS_INFO_STREAM("angle_axis_ay = "<<temp_parameters.angle_axis[1]);
00165     ROS_INFO_STREAM("angle_axis_az =  "<<temp_parameters.angle_axis[2]);
00166     ROS_INFO_STREAM("position_x =  "<<temp_parameters.position[0]);
00167     ROS_INFO_STREAM("position_y =  "<<temp_parameters.position[1]);
00168     ROS_INFO_STREAM("position_z =  "<<temp_parameters.position[2]);
00169     ROS_INFO_STREAM("focal_length_x =  "<<temp_parameters.focal_length_x);
00170     ROS_INFO_STREAM("focal_length_y =  "<<temp_parameters.focal_length_y);
00171     ROS_INFO_STREAM("center_x = "<<temp_parameters.center_x);
00172     ROS_INFO_STREAM("center_y =  "<<temp_parameters.center_y);
00173     ROS_INFO_STREAM("distortion_k1 =  "<<temp_parameters.distortion_k1);
00174     ROS_INFO_STREAM("distortion_k2 =  "<<temp_parameters.distortion_k2);
00175     ROS_INFO_STREAM("distortion_k3 =  "<<temp_parameters.distortion_k3);
00176     ROS_INFO_STREAM("distortion_p1 =  "<<temp_parameters.distortion_p1);
00177     ROS_INFO_STREAM("distortion_p2 =  "<<temp_parameters.distortion_p2);
00178     ROS_INFO_STREAM("scene_id = "<<scene_id);*/
00179     ROS_ERROR("load() Failed to read in cameras yaml file");
00180     ROS_ERROR_STREAM("Failed with exception "<< e.what());
00181     return (false);
00182   }
00183   return true;
00184 }
00185 
00186 bool CalibrationJob::loadTarget()
00187 {
00188   std::ifstream target_input_file(target_def_file_name_.c_str());
00189   if (target_input_file.fail())
00190   {
00191     ROS_ERROR_STREAM(
00192         "ERROR CalibrationJob::load(), couldn't open target_input_file: "
00193         << target_def_file_name_.c_str());
00194     return (false);
00195   }
00196   Target temp_target;
00197   std::string temp_frame;
00198   try
00199   {
00200     YAML::Parser target_parser(target_input_file);
00201     YAML::Node target_doc;
00202     target_parser.GetNextDocument(target_doc);
00203     ROS_INFO_STREAM("Parsing Target file...");
00204     // read in all static targets
00205     if (const YAML::Node *target_parameters = target_doc.FindValue("static_targets"))
00206     {
00207       ROS_DEBUG_STREAM("Found "<<target_parameters->size() <<" targets ");
00208       shared_ptr<Target> temp_target = make_shared<Target>();
00209       temp_target->is_moving = false;
00210       for (unsigned int i = 0; i < target_parameters->size(); i++)
00211       {
00212         (*target_parameters)[i]["target_name"] >> temp_target->target_name;
00213         (*target_parameters)[i]["target_frame"] >> temp_frame;
00214         (*target_parameters)[i]["target_type"] >> temp_target->target_type;
00215         //ROS_DEBUG_STREAM("TargetFrame: "<<temp_frame);
00216         switch (temp_target->target_type)
00217         {
00218           case pattern_options::Chessboard:
00219             (*target_parameters)[i]["target_rows"] >> temp_target->checker_board_parameters.pattern_rows;
00220             (*target_parameters)[i]["target_cols"] >> temp_target->checker_board_parameters.pattern_cols;
00221             ROS_DEBUG_STREAM("TargetRows: "<<temp_target->checker_board_parameters.pattern_rows);
00222             break;
00223           case pattern_options::CircleGrid:
00224             (*target_parameters)[i]["target_rows"] >> temp_target->circle_grid_parameters.pattern_rows;
00225             (*target_parameters)[i]["target_cols"] >> temp_target->circle_grid_parameters.pattern_cols;
00226             temp_target->circle_grid_parameters.is_symmetric=true;
00227             ROS_DEBUG_STREAM("TargetRows: "<<temp_target->circle_grid_parameters.pattern_rows);
00228             break;
00229           default:
00230             ROS_ERROR_STREAM("target_type does not correlate to a known pattern option (Chessboard or CircleGrid)");
00231             return false;
00232             break;
00233         }
00234         (*target_parameters)[i]["angle_axis_ax"] >> temp_target->pose.ax;
00235         (*target_parameters)[i]["angle_axis_ay"] >> temp_target->pose.ay;
00236         (*target_parameters)[i]["angle_axis_az"] >> temp_target->pose.az;
00237         (*target_parameters)[i]["position_x"] >> temp_target->pose.x;
00238         (*target_parameters)[i]["position_y"] >> temp_target->pose.y;
00239         (*target_parameters)[i]["position_z"] >> temp_target->pose.z;
00240         (*target_parameters)[i]["num_points"] >> temp_target->num_points;
00241         const YAML::Node *points_node = (*target_parameters)[i].FindValue("points");
00242         ROS_DEBUG_STREAM("FoundPoints: "<<points_node->size());
00243         for (int j = 0; j < points_node->size(); j++)
00244         {
00245           const YAML::Node *pnt_node = (*points_node)[j].FindValue("pnt");
00246           std::vector<float> temp_pnt;
00247           (*pnt_node) >> temp_pnt;
00248           Point3d temp_pnt3d;
00249           //ROS_DEBUG_STREAM("pntx: "<<temp_pnt3d.x);
00250           temp_pnt3d.x = temp_pnt[0];
00251           temp_pnt3d.y = temp_pnt[1];
00252           temp_pnt3d.z = temp_pnt[2];
00253           temp_target->pts.push_back(temp_pnt3d);
00254         }
00255         ceres_blocks_.addStaticTarget(temp_target);
00256         target_frames_.push_back(temp_frame);
00257       }
00258     }
00259 
00260     // read in all moving targets
00261     if (const YAML::Node *target_parameters = target_doc.FindValue("moving_targets"))
00262     {
00263       ROS_DEBUG_STREAM("Found "<<target_parameters->size() <<"  moving targets ");
00264       shared_ptr<Target> temp_target = make_shared<Target>();
00265       unsigned int scene_id;
00266       temp_target->is_moving = true;
00267       for (unsigned int i = 0; i < target_parameters->size(); i++)
00268       {
00269         (*target_parameters)[i]["target_name"] >> temp_target->target_name;
00270         (*target_parameters)[i]["target_frame"] >> temp_frame;
00271         (*target_parameters)[i]["target_type"] >> temp_target->target_type;
00272         //ROS_DEBUG_STREAM("TargetFrame: "<<temp_frame);
00273         switch (temp_target->target_type)
00274         {
00275           case pattern_options::Chessboard:
00276             (*target_parameters)[i]["target_rows"] >> temp_target->checker_board_parameters.pattern_rows;
00277             (*target_parameters)[i]["target_cols"] >> temp_target->checker_board_parameters.pattern_cols;
00278             ROS_INFO_STREAM("TargetRows: "<<temp_target->checker_board_parameters.pattern_rows);
00279             break;
00280           case pattern_options::CircleGrid:
00281             (*target_parameters)[i]["target_rows"] >> temp_target->circle_grid_parameters.pattern_rows;
00282             (*target_parameters)[i]["target_cols"] >> temp_target->circle_grid_parameters.pattern_cols;
00283             temp_target->circle_grid_parameters.is_symmetric=true;
00284             break;
00285           default:
00286             ROS_ERROR_STREAM("target_type does not correlate to a known pattern option (Chessboard or CircleGrid)");
00287             return false;
00288             break;
00289         }
00290         (*target_parameters)[i]["angle_axis_ax"] >> temp_target->pose.ax;
00291         (*target_parameters)[i]["angle_axis_ax"] >> temp_target->pose.ay;
00292         (*target_parameters)[i]["angle_axis_ay"] >> temp_target->pose.az;
00293         (*target_parameters)[i]["position_x"] >> temp_target->pose.x;
00294         (*target_parameters)[i]["position_y"] >> temp_target->pose.y;
00295         (*target_parameters)[i]["position_z"] >> temp_target->pose.z;
00296         (*target_parameters)[i]["scene_id"] >> scene_id;
00297         (*target_parameters)[i]["num_points"] >> temp_target->num_points;
00298         const YAML::Node *points_node = (*target_parameters)[i].FindValue("points");
00299         for (int j = 0; j < points_node->size(); j++)
00300         {
00301           const YAML::Node *pnt_node = (*points_node)[j].FindValue("pnt");
00302           std::vector<float> temp_pnt;
00303           (*pnt_node) >> temp_pnt;
00304           Point3d temp_pnt3d;
00305           temp_pnt3d.x = temp_pnt[0];
00306           temp_pnt3d.y = temp_pnt[1];
00307           temp_pnt3d.z = temp_pnt[2];
00308           temp_target->pts.push_back(temp_pnt3d);
00309         }
00310         ceres_blocks_.addMovingTarget(temp_target, scene_id);
00311         target_frames_.push_back(temp_frame);
00312       }
00313     }
00314   } // end try
00315   catch (YAML::ParserException& e)
00316   {
00317     ROS_ERROR("load() Failed to read in target yaml file");
00318     ROS_ERROR_STREAM("Failed with exception "<< e.what());
00319     return (false);
00320   }
00321   return true;
00322 
00323 }
00324 
00325 bool CalibrationJob::loadCalJob()
00326 {
00327   std::ifstream caljob_input_file(caljob_def_file_name_.c_str());
00328   if (caljob_input_file.fail())
00329   {
00330     ROS_ERROR_STREAM(
00331         "ERROR CalibrationJob::load(), couldn't open caljob_input_file: "
00332         << caljob_def_file_name_.c_str());
00333     return (false);
00334   }
00335 
00336   std::string trigger_message="triggered";//TODO what's in the message?
00337   std::string opt_params;
00338   int scene_id_num;
00339   int trig_type;
00340   Trigger cal_trig;
00341   cal_trig.trigger_popup_msg=trigger_message;
00342   std::string camera_name;
00343   shared_ptr<Camera> temp_cam = make_shared<Camera>();
00344   shared_ptr<Target> temp_targ = make_shared<Target>();
00345   Roi temp_roi;
00346 
00347   try
00348   {
00349     YAML::Parser caljob_parser(caljob_input_file);
00350     YAML::Node caljob_doc;
00351     caljob_parser.GetNextDocument(caljob_doc);
00352 
00353     caljob_doc["reference_frame"] >> reference_frame_;
00354     caljob_doc["optimization_parameters"] >> opt_params;
00355     // read in all scenes
00356     if (const YAML::Node *caljob_scenes = caljob_doc.FindValue("scenes"))
00357     {
00358       ROS_DEBUG_STREAM("Found "<<caljob_scenes->size() <<" scenes");
00359       scene_list_.resize(caljob_scenes->size() );
00360       for (unsigned int i = 0; i < caljob_scenes->size(); i++)
00361       {
00362         (*caljob_scenes)[i]["scene_id"] >> scene_id_num;
00363         //ROS_INFO_STREAM("scene "<<scene_id_num);
00364         (*caljob_scenes)[i]["trigger_type"] >> trig_type;
00365         //ROS_INFO_STREAM("trig type "<<trig_type);
00366         cal_trig.trigger_type=trig_type;
00367         scene_list_.at(i).setTrig(cal_trig);
00368         scene_list_.at(i).setSceneId(scene_id_num);
00369         const YAML::Node *obs_node = (*caljob_scenes)[i].FindValue("observations");
00370         ROS_DEBUG_STREAM("Found "<<obs_node->size() <<" observations within scene "<<i);
00371         for (unsigned int j = 0; j < obs_node->size(); j++)
00372         {
00373           //ROS_INFO_STREAM("For obs "<<j);
00374           (*obs_node)[j]["camera"] >> camera_name;
00375           temp_cam = ceres_blocks_.getCameraByName(camera_name);
00376 
00377           scene_list_.at(i).addCameraToScene(temp_cam);
00378 
00379           (*obs_node)[j]["roi_x_min"] >> temp_roi.x_min;
00380           (*obs_node)[j]["roi_x_max"] >> temp_roi.x_max;
00381           (*obs_node)[j]["roi_y_min"] >> temp_roi.y_min;
00382           (*obs_node)[j]["roi_y_max"] >> temp_roi.y_max;
00383           (*obs_node)[j]["target"] >> temp_targ->target_name;
00384           temp_targ = ceres_blocks_.getTargetByName(temp_targ->target_name);
00385 
00386           scene_list_.at(i).populateObsCmdList(temp_cam, temp_targ, temp_roi);
00387         }
00388       }
00389     }
00390   } // end try
00391   catch (YAML::ParserException& e)
00392   {
00393     ROS_ERROR("load() Failed to read in caljob yaml file");
00394     ROS_ERROR_STREAM("Failed with exception "<< e.what());
00395     return (false);
00396   }
00397   return true;
00398 }
00399 
00400 bool CalibrationJob::run()
00401 {
00402   runObservations();
00403   runOptimization();
00404   return true;
00405 }
00406 
00407 bool CalibrationJob::runObservations()
00408 {
00409   ROS_DEBUG_STREAM("Running observations...");
00410   this->ceres_blocks_.clearCamerasTargets();
00411   // For each scene
00412   BOOST_FOREACH(ObservationScene current_scene, scene_list_)
00413   {
00414     int scene_id = current_scene.get_id();
00415 
00416     // clear all observations from every camera
00417     ROS_DEBUG_STREAM("Processing Scene " << scene_id<<" of "<< scene_list_.size());
00418 
00419     // add observations to every camera
00420     //ROS_INFO_STREAM("Processing " << current_scene.cameras_in_scene_.size() <<" Cameras ");
00421     BOOST_FOREACH(shared_ptr<Camera> current_camera, current_scene.cameras_in_scene_)
00422     {
00423       //ROS_INFO_STREAM("Current Camera name: "<<current_camera->camera_name_);
00424       current_camera->camera_observer_->clearObservations(); // clear any recorded data
00425       current_camera->camera_observer_->clearTargets(); // clear all targets
00426     }
00427 
00428     // add each target to each cameras observations
00429     ROS_DEBUG_STREAM("Processing " << current_scene.observation_command_list_.size()
00430                      <<" Observation Commands");
00431     BOOST_FOREACH(ObservationCmd o_command, current_scene.observation_command_list_)
00432     {
00433       // configure to find target in roi
00434       o_command.camera->camera_observer_->addTarget(o_command.target, o_command.roi);
00435       //ROS_INFO_STREAM("Current Camera name: "<<o_command.camera->camera_name_);
00436       //ROS_INFO_STREAM("Current Target name: "<<o_command.target->target_name);
00437       //ROS_INFO_STREAM("Current roi xmin: "<<o_command.roi.x_min);
00438     }
00439     // trigger the cameras
00440     BOOST_FOREACH( shared_ptr<Camera> current_camera, current_scene.cameras_in_scene_)
00441     {
00442       current_camera->camera_observer_->triggerCamera();
00443     }
00444     // collect results
00445     P_BLOCK intrinsics;
00446     P_BLOCK extrinsics;
00447     P_BLOCK target_pose;
00448     P_BLOCK pnt_pos;
00449     std::string camera_name;
00450     std::string target_name;
00451     /*ROS_INFO_STREAM("static camera extrinsics: "<<ceres_blocks_.static_cameras_.at(0)->camera_parameters_.angle_axis[0]<<" "
00452                              <<ceres_blocks_.static_cameras_.at(0)->camera_parameters_.angle_axis[1]<<" "
00453                              <<ceres_blocks_.static_cameras_.at(0)->camera_parameters_.angle_axis[2]);*/
00454 
00455     // for each camera in scene
00456       ObservationDataPointList listpercamera;
00457     BOOST_FOREACH( shared_ptr<Camera> camera, current_scene.cameras_in_scene_)
00458     {
00459 
00460       // wait until observation is done
00461       while (!camera->camera_observer_->observationsDone())
00462         ;
00463 
00464       camera_name = camera->camera_name_;
00465       if (camera->isMoving())
00466       {
00467         // next line does nothing if camera already exist in blocks
00468         ceres_blocks_.addMovingCamera(camera, scene_id);
00469         intrinsics = ceres_blocks_.getMovingCameraParameterBlockIntrinsics(camera_name);
00470         extrinsics = ceres_blocks_.getMovingCameraParameterBlockExtrinsics(camera_name, scene_id);
00471       }
00472       else
00473       {
00474         // next line does nothing if camera already exist in blocks
00475         ceres_blocks_.addStaticCamera(camera);
00476         intrinsics = ceres_blocks_.getStaticCameraParameterBlockIntrinsics(camera_name);
00477         extrinsics = ceres_blocks_.getStaticCameraParameterBlockExtrinsics(camera_name);
00478       }
00479 
00480       // Get the observations
00481       CameraObservations camera_observations;
00482       int number_returned;
00483       number_returned = camera->camera_observer_->getObservations(camera_observations);
00484 
00485       ROS_DEBUG_STREAM("Processing " << camera_observations.observations.size()
00486                            <<" Observations");
00487       BOOST_FOREACH(Observation observation, camera_observations.observations)
00488       {
00489         target_name = observation.target->target_name;
00490         int pnt_id = observation.point_id;
00491         double observation_x = observation.image_loc_x;
00492         double observation_y = observation.image_loc_y;
00493         if (observation.target->is_moving)
00494         {
00495           ceres_blocks_.addMovingTarget(observation.target, scene_id);
00496           target_pose = ceres_blocks_.getMovingTargetPoseParameterBlock(target_name, scene_id);
00497           pnt_pos = ceres_blocks_.getMovingTargetPointParameterBlock(target_name, pnt_id);
00498         }
00499         else
00500         {
00501           ceres_blocks_.addStaticTarget(observation.target); // if exist, does nothing
00502           target_pose = ceres_blocks_.getStaticTargetPoseParameterBlock(target_name);
00503           pnt_pos = ceres_blocks_.getStaticTargetPointParameterBlock(target_name, pnt_id);
00504         }
00505         ObservationDataPoint temp_ODP(camera_name, target_name, scene_id, intrinsics, extrinsics, pnt_id, target_pose,
00506                                       pnt_pos, observation_x, observation_y);
00507         listpercamera.addObservationPoint(temp_ODP);
00508       }//end for each observed point
00509     }//end for each camera
00510     observation_data_point_list_.push_back(listpercamera);
00511   } //end for each scene
00512   return true;
00513 }
00514 
00515 bool CalibrationJob::runOptimization()
00516 {
00517   // take all the data collected and create a Ceres optimization problem and run it
00518   ROS_INFO_STREAM("Running Optimization...");
00519   ROS_DEBUG_STREAM("Optimizing "<<scene_list_.size()<<" scenes");
00520       BOOST_FOREACH(ObservationScene current_scene, scene_list_)
00521       {
00522     int scene_id = current_scene.get_id();
00523 
00524     //ROS_DEBUG_STREAM("Optimizing # cameras: "<<current_scene.cameras_in_scene_);
00525 
00526     BOOST_FOREACH(shared_ptr<Camera> camera, current_scene.cameras_in_scene_)
00527     {
00528 
00529     ROS_DEBUG_STREAM("Current observation data point list size: "<<observation_data_point_list_.at(scene_id).items.size());
00530     // take all the data collected and create a Ceres optimization problem and run it
00531     P_BLOCK extrinsics;
00532     P_BLOCK target_pose;
00533     BOOST_FOREACH(ObservationDataPoint ODP, observation_data_point_list_.at(scene_id).items)
00534     {
00535       // create cost function
00536       // there are several options
00537       // 1. the complete reprojection error cost function "Create(obs_x,obs_y)"
00538       //    this cost function has the following parameters:
00539       //      a. camera intrinsics
00540       //      b. camera extrinsics
00541       //      c. target pose
00542       //      d. point location in target frame
00543       // 2. the same as 1, but without d  "Create(obs_x,obs_y,t_pnt_x, t_pnt_y, t_pnt_z)
00544       // 3. the same as 1, but without a  "Create(obs_x,obs_y,fx,fy,cx,cy,cz)"
00545       //    Note that this one assumes we are using rectified images to compute the observations
00546       // 4. the same as 3, point location fixed too "Create(obs_x,obs_y,fx,fy,cx,cy,cz,t_x,t_y,t_z)"
00547       //        implemented in TargetCameraReprjErrorNoDistortion
00548       // 5. the same as 4, but with target in known location
00549       //    "Create(obs_x,obs_y,fx,fy,cx,cy,cz,t_x,t_y,t_z,p_tx,p_ty,p_tz,p_ax,p_ay,p_az)"
00550 
00551 
00552       // pull out the constants from the observation point data
00553       double focal_length_x = ODP.camera_intrinsics_[0]; // TODO, make this not so ugly
00554       double focal_length_y = ODP.camera_intrinsics_[1];
00555       double center_pnt_x   = ODP.camera_intrinsics_[2];
00556       double center_pnt_y   = ODP.camera_intrinsics_[3];
00557       double image_x        = ODP.image_x_;
00558       double image_y        = ODP.image_y_;
00559       double point_x        = ODP.point_position_[0];// location of point within target frame
00560       double point_y        = ODP.point_position_[1];
00561       double point_z        = ODP.point_position_[2];
00562 
00563       // create the cost function
00564       CostFunction* cost_function = TargetCameraReprjErrorNoDistortion::Create(image_x, image_y,
00565                                                                                focal_length_x,
00566                                                                                focal_length_y,
00567                                                                                center_pnt_x,
00568                                                                                center_pnt_y,
00569                                                                                point_x,
00570                                                                                point_y,
00571                                                                                point_z);
00572 
00573       // pull out pointers to the parameter blocks in the observation point data
00574       extrinsics    = ODP.camera_extrinsics_;
00575       target_pose   = ODP.target_pose_;
00576 
00577       // add it as a residual using parameter blocks
00578       problem_.AddResidualBlock(cost_function, NULL , extrinsics, target_pose);
00579     }//for each observation
00580       problem_.SetParameterBlockConstant(target_pose);
00581     // Make Ceres automatically detect the bundle structure. Note that the
00582     // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
00583     // for standard bundle adjustment problems.
00584     ceres::Solver::Options options;
00585     options.linear_solver_type = ceres::DENSE_SCHUR;
00586     options.minimizer_progress_to_stdout = false;
00587     options.max_num_iterations = 1000;
00588 
00589     ceres::Solver::Summary summary;
00590     ceres::Solve(options, &problem_, &summary);
00591     extrinsics_.push_back(extrinsics);
00592     target_pose_.push_back(target_pose);
00593 
00594     //return true;
00595     }//for each camera
00596   }//for each scene
00597   return true;
00598 }//end runOptimization
00599 
00600 bool CalibrationJob::store()
00601 {
00602   std::string path = ros::package::getPath("industrial_extrinsic_cal");
00603   std::string file_path = "/launch/target_to_camera_optical_transform_publisher.launch";
00604   std::string filepath = path+file_path;
00605   std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
00606   if (output_file.is_open())
00607   {
00608     ROS_INFO_STREAM("Storing results in: "<<filepath);
00609   }
00610   else
00611   {
00612     ROS_ERROR_STREAM("Unable to open file");
00613     return false;
00614   }//end if writing to file
00615   output_file << "<launch>";
00616   for (int i=0; i<extrinsics_.size();i++)
00617   {
00618     output_file << "\n";
00619     //get transform world to camera
00620     double R[9];
00621     double aa[3];
00622     aa[0] = extrinsics_.at(i)[0];
00623     aa[1] = extrinsics_.at(i)[1];
00624     aa[2] = extrinsics_.at(i)[2];
00625     double tx=extrinsics_.at(i)[3];
00626     double ty=extrinsics_.at(i)[4];
00627     double tz=extrinsics_.at(i)[5];
00628     ceres::AngleAxisToRotationMatrix(aa, R);
00629     double ix = -(tx * R[0] + ty * R[1] + tz * R[2]);
00630     double iy = -(tx * R[3] + ty * R[4] + tz * R[5]);
00631     double iz = -(tx * R[6] + ty * R[7] + tz * R[8]);
00632     double rx = atan2(R[7], R[8]);
00633     double ry = atan2(-R[6], sqrt(R[7] * R[7] + R[8] * R[8]));
00634     double rz = atan2(R[3], R[0]);
00635 
00636     output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"camera_tf_broadcaster"<<i<<"\" args=\"";
00637     //tranform publisher launch files requires x y z yaw pitch roll
00638     output_file<<ix<< ' '<<iy<< ' '<<iz<< ' '<<rz<< ' '<<ry<< ' '<<rx ;
00639     output_file<<" "<<target_frames_[i];
00640     output_file<<" "<<camera_optical_frames_[i];
00641     output_file<<" 100\" />";
00642   }//end for loop # extrinsics
00643   output_file << "\n</launch> \n";
00644   output_file.close();
00645   return true;
00646 }
00647 
00648 }//end namespace industrial_extrinsic_cal


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