ceres_blocks.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 
00020 #include <industrial_extrinsic_cal/ceres_blocks.h>
00021 #include <boost/shared_ptr.hpp>
00022 
00023 using std::string;
00024 using boost::shared_ptr;
00025 
00026 namespace industrial_extrinsic_cal
00027 {
00028 CeresBlocks::CeresBlocks()
00029 {
00030 }
00031 CeresBlocks::~CeresBlocks()
00032 {
00033   clearCamerasTargets();
00034 }
00035 void CeresBlocks::clearCamerasTargets()
00036 {
00037   //ROS_INFO_STREAM("Attempting to clear cameras and targets from ceresBlocks");
00038   static_targets_.clear();
00039   //ROS_INFO_STREAM("Moving Targets "<<moving_targets_.size());
00040   moving_targets_.clear();
00041   //ROS_INFO_STREAM("Static cameras "<<static_cameras_.size());
00042   static_cameras_.clear();
00043   //ROS_INFO_STREAM("Moving cameras "<<moving_cameras_.size());
00044   moving_cameras_.clear();
00045   //ROS_INFO_STREAM("Cameras and Targets cleared from CeresBlocks");
00046 }
00047 P_BLOCK CeresBlocks::getStaticCameraParameterBlockIntrinsics(string camera_name)
00048 {
00049   // static cameras should have unique name
00050   BOOST_FOREACH(shared_ptr<Camera> camera, static_cameras_)
00051   {
00052     if (camera_name == camera->camera_name_)
00053     {
00054       P_BLOCK intrinsics = &(camera->camera_parameters_.pb_intrinsics[0]);
00055       return (intrinsics);
00056     }
00057   }
00058   return (NULL);
00059 }
00060 P_BLOCK CeresBlocks::getMovingCameraParameterBlockIntrinsics(string camera_name)
00061 {
00062   // we use the intrinsic parameters from the first time the camera appears in the list
00063   // subsequent cameras with this name also have intrinsic parameters, but these are
00064   // never used as parameter blocks, only their extrinsics are used
00065   BOOST_FOREACH(shared_ptr<MovingCamera> moving_camera, moving_cameras_)
00066   {
00067     if (camera_name == moving_camera->cam->camera_name_)
00068     {
00069       P_BLOCK intrinsics = &(moving_camera->cam->camera_parameters_.pb_intrinsics[0]);
00070       return (intrinsics);
00071     }
00072   }
00073   return (NULL);
00074 }
00075 P_BLOCK CeresBlocks::getStaticCameraParameterBlockExtrinsics(string camera_name)
00076 {
00077   // static cameras should have unique name
00078   BOOST_FOREACH(shared_ptr<Camera> camera, static_cameras_)
00079   {
00080     if (camera_name == camera->camera_name_)
00081     {
00082       P_BLOCK extrinsics = &(camera->camera_parameters_.pb_extrinsics[0]);
00083       return (extrinsics);
00084     }
00085   }
00086   return (NULL);
00087 
00088 }
00089 P_BLOCK CeresBlocks::getMovingCameraParameterBlockExtrinsics(string camera_name, int scene_id)
00090 {
00091   BOOST_FOREACH(shared_ptr<MovingCamera> camera, moving_cameras_)
00092   {
00093     if (camera_name == camera->cam->camera_name_ && scene_id == camera->scene_id)
00094     {
00095       P_BLOCK extrinsics = &(camera->cam->camera_parameters_.pb_extrinsics[0]);
00096       return (extrinsics);
00097     }
00098   }
00099   return (NULL);
00100 
00101 }
00102 P_BLOCK CeresBlocks::getStaticTargetPoseParameterBlock(string target_name)
00103 {
00104   BOOST_FOREACH(shared_ptr<Target> target, static_targets_)
00105   {
00106     if (target_name == target->target_name)
00107     {
00108       P_BLOCK pose = &(target->pose.pb_pose[0]);
00109       return (pose);
00110     }
00111   }
00112   return (NULL);
00113 }
00114 P_BLOCK CeresBlocks::getStaticTargetPointParameterBlock(string target_name, int point_id)
00115 {
00116   BOOST_FOREACH(shared_ptr<Target> target, static_targets_)
00117   {
00118     if (target_name == target->target_name)
00119     {
00120       P_BLOCK point_position = &(target->pts[point_id].pb[0]);
00121       return (point_position);
00122     }
00123   }
00124   return (NULL);
00125 }
00126 P_BLOCK CeresBlocks::getMovingTargetPoseParameterBlock(string target_name, int scene_id)
00127 {
00128   BOOST_FOREACH(shared_ptr<MovingTarget> moving_target, moving_targets_)
00129   {
00130     if (target_name == moving_target->targ->target_name && scene_id == moving_target->scene_id)
00131     {
00132       P_BLOCK pose = &(moving_target->targ->pose.pb_pose[0]);
00133       return (pose);
00134     }
00135   }
00136   return (NULL);
00137 }
00138 P_BLOCK CeresBlocks::getMovingTargetPointParameterBlock(string target_name, int pnt_id)
00139 {
00140   // note scene_id unnecessary here since regarless of scene th point's location relative to
00141   // the target frame does not change
00142   BOOST_FOREACH(shared_ptr<MovingTarget> moving_target, moving_targets_)
00143   {
00144     if (target_name == moving_target->targ->target_name)
00145     {
00146       P_BLOCK point_position = &(moving_target->targ->pts[pnt_id].pb[0]);
00147       return (point_position);
00148     }
00149   }
00150   return (NULL);
00151 }
00152 
00153 bool CeresBlocks::addStaticCamera(shared_ptr<Camera> camera_to_add)
00154 {
00155   BOOST_FOREACH(shared_ptr<Camera> cam, static_cameras_)
00156   {
00157     if (cam->camera_name_ == camera_to_add->camera_name_)
00158       return (false); // camera already exists
00159   }
00160   static_cameras_.push_back(camera_to_add);
00161   //ROS_INFO_STREAM("Camera added to static_cameras_");
00162   return (true);
00163 }
00164 bool CeresBlocks::addStaticTarget(shared_ptr<Target> target_to_add)
00165 {
00166   BOOST_FOREACH(shared_ptr<Target> targ, static_targets_)
00167   {
00168     if (targ->target_name == target_to_add->target_name)
00169       return (false); // target already exists
00170   }
00171   static_targets_.push_back(target_to_add);
00172   return (true);
00173 }
00174 bool CeresBlocks::addMovingCamera(shared_ptr<Camera> camera_to_add, int scene_id)
00175 {
00176   BOOST_FOREACH(shared_ptr<MovingCamera> cam, moving_cameras_)
00177   {
00178     if (cam->cam->camera_name_ == camera_to_add->camera_name_ && cam->scene_id == scene_id)
00179       return (false); // camera already exists
00180   }
00181   // this next line allocates the memory for a moving camera
00182   shared_ptr<MovingCamera> temp_moving_camera = boost::make_shared<MovingCamera>();
00183   // this next line allocates the memory for the actual camera
00184   shared_ptr<Camera> temp_camera = boost::make_shared<Camera>(camera_to_add->camera_name_, camera_to_add->camera_parameters_,
00185                                                        true);
00186   temp_moving_camera->cam = temp_camera;
00187   temp_moving_camera->scene_id = scene_id;
00188   moving_cameras_.push_back(temp_moving_camera);
00189   return (true);
00190 }
00191 bool CeresBlocks::addMovingTarget(shared_ptr<Target> target_to_add, int scene_id)
00192 {
00193   BOOST_FOREACH(shared_ptr<MovingTarget> targ, moving_targets_)
00194   {
00195     if (targ->targ->target_name == target_to_add->target_name && targ->scene_id == scene_id)
00196       return (false); // target already exists
00197   }
00198   shared_ptr<MovingTarget> temp_moving_target = boost::make_shared<MovingTarget>();
00199   shared_ptr<Target> temp_camera = boost::make_shared<Target>();
00200   temp_moving_target->targ = target_to_add;
00201   temp_moving_target->scene_id = scene_id;
00202   moving_targets_.push_back(temp_moving_target);
00203   return (true);
00204 }
00205 
00206 const boost::shared_ptr<Camera> CeresBlocks::getCameraByName(const std::string &camera_name)
00207 {
00208   boost::shared_ptr<Camera> cam = boost::make_shared<Camera>();
00209   //ROS_INFO_STREAM("Found "<<static_cameras_.size() <<" static cameras");
00210   for (int i=0; i< static_cameras_.size() ; i++ )
00211   {
00212     if (static_cameras_.at(i)->camera_name_==camera_name)
00213     {
00214       cam= static_cameras_.at(i);
00215       ROS_DEBUG_STREAM("Found static camera with name: "<<static_cameras_.at(i)->camera_name_);
00216     }
00217   }
00218   //ROS_INFO_STREAM("Found "<<moving_cameras_.size() <<" moving cameras");
00219   for (int i=0; i< moving_cameras_.size() ; i++ )
00220   {
00221     if (moving_cameras_.at(i)->cam->camera_name_==camera_name)
00222     {
00223       cam= moving_cameras_.at(i)->cam;
00224       ROS_DEBUG_STREAM("Found moving camera with name: "<<camera_name);
00225     }
00226   }
00227   if (!cam)
00228   {
00229     ROS_ERROR_STREAM("Fail");
00230   }
00231   return cam;
00232   //return true;
00233 }
00234 
00235 const boost::shared_ptr<Target> CeresBlocks::getTargetByName(const std::string &target_name)
00236 {
00237   boost::shared_ptr<Target> target = boost::make_shared<Target>();
00238   //ROS_INFO_STREAM("Found "<<static_cameras_.size() <<" static cameras");
00239   for (int i=0; i< static_targets_.size() ; i++ )
00240   {
00241     if (static_targets_.at(i)->target_name==target_name)
00242     {
00243       target=static_targets_.at(i);
00244       ROS_DEBUG_STREAM("Found static target with name: "<<target_name);
00245     }
00246   }
00247   //ROS_INFO_STREAM("Found "<<moving_cameras_.size() <<" static cameras");
00248   for (int i=0; i< moving_targets_.size() ; i++ )
00249   {
00250     if (moving_targets_.at(i)->targ->target_name==target_name)
00251     {
00252       target=moving_targets_.at(i)->targ;
00253       ROS_DEBUG_STREAM("Found moving target with name: "<<target_name);
00254     }
00255   }
00256   if (!target)
00257   {
00258     ROS_ERROR_STREAM("Fail");
00259   }
00260   return target;
00261 }
00262 
00263 
00264 }// end namespace industrial_extrinsic_cal
00265 
00266 


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