00001 #include "MarkersConfig.h"
00002 #include <vector>
00003 #include <iostream>
00004 #include <fstream>
00005 #include <sstream>
00006 #include <opencv2/core/core.hpp>
00007 #include <list>
00008 #include <ros/ros.h>
00009 #include <cstdio>
00010
00011
00012
00013 MarkersConfig::MarkersConfig(float default_val):
00014 msize(__number_of_possible_markers,default_val),
00015 mposition(__number_of_possible_markers,cv::Point3f(0,0,0))
00016 {
00017
00018 }
00019
00020 MarkersConfig::MarkersConfig(std::string path, float default_val)throw(cv::Exception):
00021 msize(__number_of_possible_markers,default_val),
00022 mposition(__number_of_possible_markers,cv::Point3f(0,0,0))
00023 {
00024 std::ifstream file(path.c_str());
00025 if(!file) throw cv::Exception(9100, "Could not open file:"+path,"::constructor::",__FILE__,__LINE__);
00026
00027 std::list<int> readIDs;
00028 char line[1024];
00029 while(!file.eof()){
00030 file.getline(line,1024);
00031 int id;
00032 float size,m_x,m_y,m_z;
00033 switch (sscanf(line,"%d = %f (%f,%f,%f)",&id,&size,&m_x,&m_y,&m_z)){
00034 case 2:
00035 if(this->msize.at(id) != default_val){
00036 ROS_WARN_STREAM("Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id));
00037 }
00038
00039 this->msize.at(id)=size;
00040 readIDs.push_back(id);
00041
00042
00043 ROS_INFO_STREAM(id << " = " << size );
00044 break;
00045 case 5:
00046 if(this->msize.at(id) != default_val){
00047 std::cout << "Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id) << std::endl;
00048 std::cout << "Probably you want to review the file configuration: " << std::endl;
00049 }
00050 this->msize.at(id)=size;
00051 this->mposition.at(id)= cv::Point3f(m_x,m_y,m_z);
00052 readIDs.push_back(id);
00053
00054
00055 ROS_INFO_STREAM(id << " = " << size
00056 << " (" << m_x
00057 << ',' << m_y
00058 << ',' << m_z << ')');
00059 break;
00060
00061 }
00062
00063 }
00064 }
00065
00066 void MarkersConfig::parse_from_file(std::string path, float default_val)throw(cv::Exception)
00067 {
00068 msize = std::vector<float>(__number_of_possible_markers,default_val);
00069 mposition = std::vector<cv::Point3f>(__number_of_possible_markers,cv::Point3f(0,0,0));
00070
00071 std::ifstream file(path.c_str());
00072 if(!file) throw cv::Exception(9100, "Could not open file:"+path,"::constructor::",__FILE__,__LINE__);
00073
00074 std::list<int> readIDs;
00075 char line[1024];
00076 while(!file.eof()){
00077 file.getline(line,1024);
00078 int id;
00079 float size,m_x,m_y,m_z;
00080 switch (sscanf(line,"%d = %f (%f,%f,%f)",&id,&size,&m_x,&m_y,&m_z)){
00081 case 2:
00082 if(this->msize.at(id) != default_val){
00083 ROS_WARN_STREAM("Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id));
00084 }
00085
00086 this->msize.at(id)=size;
00087 readIDs.push_back(id);
00088
00089
00090 ROS_INFO_STREAM(id << " = " << size );
00091 break;
00092 case 5:
00093 if(this->msize.at(id) != default_val){
00094 ROS_WARN_STREAM("Warning: ID Marker " << id << " is " << size << " instead of the previous " << this->msize.at(id));
00095 }
00096 this->msize.at(id)=size;
00097 this->mposition.at(id)= cv::Point3f(m_x,m_y,m_z);
00098 readIDs.push_back(id);
00099
00100
00101 ROS_INFO_STREAM(id << " = " << size
00102 << " (" << m_x
00103 << ',' << m_y
00104 << ',' << m_z << ')');
00105 break;
00106
00107 }
00108
00109 }
00110 }