$search
00001 #include <iostream> 00002 //yaml reading and writing 00003 #include <opencv2/core/core.hpp> 00004 #include <fstream> 00005 #include <tf/tf.h> 00006 00007 class yamlWriter 00008 { 00009 public: 00010 int writeTrajectory(const std::string & filename, const std::vector<tf::Stamped<tf::Pose> > & opening_trajectory); 00011 00013 00014 std::string getName () const { return ("[yamlWriter: ]"); } 00015 }; 00016 00017 00018 int yamlWriter::writeTrajectory(const std::string & filename, const std::vector<tf::Stamped<tf::Pose> > & opening_trajectory) 00019 { 00020 00021 cv::FileStorage fs; 00022 fs.open(filename, cv::FileStorage::WRITE); 00023 if (!fs.isOpened()) 00024 { 00025 std::cerr << "bad yml file!" << std::endl; 00026 return -1; 00027 } 00028 00029 cv::Mat mat_trajectory; 00030 mat_trajectory = cv::Mat::zeros(opening_trajectory.size(), 7, CV_32F); 00031 for (uint i = 0; i < opening_trajectory.size(); i++) 00032 { 00033 mat_trajectory.at<float>(i, 0) = opening_trajectory[i].getOrigin().x(); 00034 mat_trajectory.at<float>(i, 1) = opening_trajectory[i].getOrigin().y(); 00035 mat_trajectory.at<float>(i, 2) = opening_trajectory[i].getOrigin().z(); 00036 mat_trajectory.at<float>(i, 3) = opening_trajectory[i].getRotation().x(); 00037 mat_trajectory.at<float>(i, 4) = opening_trajectory[i].getRotation().y(); 00038 mat_trajectory.at<float>(i, 5) = opening_trajectory[i].getRotation().z(); 00039 mat_trajectory.at<float>(i, 6) = opening_trajectory[i].getRotation().w(); 00040 } 00041 fs << "trajectory " << mat_trajectory; 00042 //cv::FileNode fn = fs["cabinet" + ss.str ()]; 00043 /* std::stringstream ss; */ 00044 /* ss << 100; */ 00045 /* cv::FileNode fn = fs["cabinet" + ss.str()]; */ 00046 /* cv::FileNodeIterator it = fn.begin(); */ 00047 /* while (it != fn.end()) */ 00048 /* { */ 00049 /* cv::FileNode n = *it; */ 00050 /* std::cerr << "n.name: " << n.name() << std::endl; */ 00051 /* if (n.name() == "type" && n.size() == 1) */ 00052 /* { */ 00053 /* // unique_id++; */ 00054 /* // cabinet.id = unique_id; */ 00055 /* std::cerr << "writting my string" << std::endl; */ 00056 /* std::string(n) == "my_string"; */ 00057 /* } */ 00058 /* } */ 00059 fs.release(); 00060 return 1; 00061 }