read_from_launch.cpp
Go to the documentation of this file.
00001 
00011 #include <ucl_drone/read_from_launch.h>
00012 
00013 // IMPROVEMENT: use NAN as default value rather than 0
00014 
00015 double Read::_img_width;
00016 double Read::_img_height;
00017 double Read::_img_center_x;
00018 double Read::_img_center_y;
00019 double Read::_focal_length_x;
00020 double Read::_focal_length_y;
00021 
00022 bool Read::CamMatrixParams(const std::string &param_name, cv::Mat &cam_matrix)
00023 {
00024   XmlRpc::XmlRpcValue cam_list;
00025   cam_matrix = cv::Mat_< double >(3, 3);
00026   // std::stringstream str_stream;
00027   if (ros::param::get(param_name, cam_list))
00028   {
00029     if (cam_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00030     {
00031       ROS_WARN("Camera matrix values for %s is not a list", param_name.c_str());
00032       return false;
00033     }
00034 
00035     if (cam_list.size() != 9)
00036     {
00037       ROS_WARN("Camera list size for %s is not of size 9 (Size: %d)", param_name.c_str(),
00038                cam_list.size());
00039       return false;
00040     }
00041 
00042     for (int32_t i = 0; i < cam_list.size(); i++)
00043     {
00044       ROS_ASSERT(cam_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00045 
00046       cam_matrix.at< double >(i % 3, i / 3) = static_cast< double >(cam_list[i]);
00047     }
00048     return true;
00049   }
00050   else
00051   {
00052     ROS_INFO("No values found for `%s` in parameters, set all to zero.", param_name.c_str());
00053     return false;
00054   }
00055 }
00056 
00057 bool Read::CamMatrixParams(const std::string &param_name)
00058 {
00059   XmlRpc::XmlRpcValue cam_list;
00060 
00061   if (ros::param::get(param_name, cam_list))
00062   {
00063     if (cam_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00064     {
00065       ROS_WARN("Camera matrix values for %s is not a list", param_name.c_str());
00066       return false;
00067     }
00068 
00069     if (cam_list.size() != 9)
00070     {
00071       ROS_WARN("Camera list size for %s is not of size 9 (Size: %d)", param_name.c_str(),
00072                cam_list.size());
00073       return false;
00074     }
00075 
00076     _img_center_x = static_cast< double >(cam_list[2]);
00077     _img_center_y = static_cast< double >(cam_list[5]);
00078     _focal_length_x = static_cast< double >(cam_list[0]);
00079     _focal_length_y = static_cast< double >(cam_list[4]);
00080 
00081     return true;
00082   }
00083   else
00084   {
00085     ROS_INFO("No values found for `%s` in parameters, set all to zero.", param_name.c_str());
00086     return false;
00087   }
00088 }
00089 
00090 bool Read::ImgSizeParams(const std::string &param_name)
00091 {
00092   XmlRpc::XmlRpcValue size_list;
00093   // std::stringstream str_stream;
00094   if (ros::param::get(param_name, size_list))
00095   {
00096     if (size_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00097     {
00098       ROS_WARN("Camera matrix values for %s is not a list", param_name.c_str());
00099       return false;
00100     }
00101 
00102     if (size_list.size() != 2)
00103     {
00104       ROS_WARN("Camera list size for %s is not of size 9 (Size: %d)", param_name.c_str(),
00105                size_list.size());
00106       return false;
00107     }
00108     // str_stream << param_name << " set to [";
00109 
00110     _img_width = static_cast< double >(size_list[0]);
00111     _img_height = static_cast< double >(size_list[1]);
00112 
00113     return true;
00114   }
00115   else
00116   {
00117     ROS_INFO("No values found for `%s` in parameters, set all to zero.", param_name.c_str());
00118     return false;
00119   }
00120 }
00121 
00122 double Read::img_width()
00123 {
00124   if (_img_width == 0)
00125   {
00126     ROS_ERROR("Read error (img_width)");
00127   }
00128   return _img_width;
00129 }
00130 
00131 double Read::img_height()
00132 {
00133   if (_img_height == 0)
00134   {
00135     ROS_ERROR("Read error (img_height)");
00136   }
00137   return _img_height;
00138 }
00139 
00140 double Read::img_center_x()
00141 {
00142   if (_img_center_x == 0)
00143   {
00144     ROS_ERROR("Read error (img_center_x)");
00145   }
00146   return _img_center_x;
00147 }
00148 
00149 double Read::img_center_y()
00150 {
00151   if (_img_center_y == 0)
00152   {
00153     ROS_ERROR("Read error (img_center_y)");
00154   }
00155   return _img_center_y;
00156 }
00157 
00158 double Read::focal_length_x()
00159 {
00160   if (_focal_length_x == 0)
00161   {
00162     ROS_ERROR("Read error (img_length_x)");
00163   }
00164   return _focal_length_x;
00165 }
00166 
00167 double Read::focal_length_y()
00168 {
00169   if (_focal_length_y == 0)
00170   {
00171     ROS_ERROR("Read error (img_length_y)");
00172   }
00173   return _focal_length_y;
00174 }


ucl_drone
Author(s): dronesinma
autogenerated on Sat Jun 8 2019 20:51:53