calibrate_camera_extrinsic_parameters.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 
3 #include <camera_info_manager/camera_info_manager.h>
4 
5 #include "opencv2/core/core.hpp"
6 #include "opencv2/imgproc/imgproc.hpp"
7 #include "opencv2/calib3d/calib3d.hpp"
8 #include "opencv2/highgui/highgui.hpp"
9 
10 #include <fstream>
11 #include <iostream>
12 #include <string>
13 
14 #include "mtt/TargetList.h"
15 
16 #include "cmath"
17 #include <iostream>
18 #include <vector>
19 
20 #include <image_transport/image_transport.h>
21 
22 #include <cv_bridge/cv_bridge.h>
23 #include <opencv/cvwimage.h>
24 
25 #include <visualization_msgs/Marker.h>
26 #include <visualization_msgs/MarkerArray.h>
27 
28 #include "sensor_msgs/LaserScan.h"
29 #include <sensor_msgs/image_encodings.h>
30 
31 #include <boost/thread.hpp>
32 #include <boost/shared_ptr.hpp>
33 
34 #include <fstream>
35 #include <string>
36 
37 using namespace std;
38 using namespace cv;
39 
40 ros::Subscriber cam_sub;
41 ros::Subscriber cam_info_sub;
42 std::string fileStorageYAML;
43 
44 std::vector<cv::Point2f> Read2DPoints(const std::string& filename);
45 std::vector<cv::Point3f> Read3DPoints(const std::string& filename);
46 
49 
50 vector<double> k_matriz;
51 
52 void cameraInfo(const sensor_msgs::CameraInfo& cam_info_msg)
53 {
54  double k;
55  for (int i=0; i<cam_info_msg.K.size(); i++)
56  {
57  k = cam_info_msg.K[i];
58  k_matriz.push_back(k);
59  }
60 
61  cam_info_sub.shutdown(); // the subscriber only runs one time
62 }
63 
64 void cameraCalibration(const sensor_msgs::ImageConstPtr& cam_msg)
65 {
66  cv_bridge::CvImagePtr cv_ptr;
67 
68  try
69  {
70  cv_ptr = cv_bridge::toCvCopy(cam_msg, sensor_msgs::image_encodings::BGR8);
71  }
72  catch (cv_bridge::Exception& e)
73  {
74  ROS_ERROR("cv_bridge exception: %s", e.what());
75  return;
76  }
77 
78  // Read points
79  std::vector<cv::Point2f> inputImage1Points = Read2DPoints(imagePoints1FileName);
80 
81  std::vector<cv::Point3f> inputObject1Points = Read3DPoints(objectPoints1FileName);
82 
83  cv::Mat initialCameraMatrix = cvCreateMat(3,3 , CV_64FC1);
84  for (int i = 0; i < 3; i++)
85  {
86  for (int j = 0; j < 3; j++)
87  {
88  initialCameraMatrix.at<double>(i, j) = k_matriz[3*i+j];
89  }
90  }
91 
92  cv::Mat distCoeffs = cvCreateMat(1,5 , CV_64FC1);
93 
94  distCoeffs.at<double>(0, 0) = 0.0;
95  distCoeffs.at<double>(0, 1) = 0.0;
96  distCoeffs.at<double>(0, 2) = 0.0;
97  distCoeffs.at<double>(0, 3) = 0.0;
98  distCoeffs.at<double>(0, 4) = 0.0;
99 
100  cv::Mat cameraMatrix;
101  cv::Mat rvecs;
102  cv::Mat tvecs;
103 
104  cameraMatrix = initialCameraMatrix;
105 
106  cv::solvePnP(inputObject1Points, inputImage1Points, cameraMatrix, distCoeffs, rvecs, tvecs);
107 
108  cv::Mat rvecs_3x3;
109  Rodrigues(rvecs, rvecs_3x3);
110 
111 // // Open yaml file
112  FileStorage fs(fileStorageYAML, FileStorage::WRITE); // xb3
113 
114  Mat R = (Mat_<double>(3,3) << rvecs.at<double>(0,0), rvecs.at<double>(0,1), rvecs.at<double>(0,2), rvecs.at<double>(1,0), rvecs.at<double>(1,1), rvecs.at<double>(1,2), rvecs.at<double>(2,0), rvecs.at<double>(2,1), rvecs.at<double>(2,2));
115 
116  Mat T = (Mat_<double>(1,3) << tvecs.at<double>(0,0), tvecs.at<double>(0,1), tvecs.at<double>(0,2));
117 
118  fs << "image_width" << cv_ptr->image.cols << "image_height" << cv_ptr->image.rows << "camera_name" << "right" << "rectification_matrix" << R << "translation_matrix" << T << "intrinsic_xb3_matrix" << "insert a rows: 3; cols: 3; dt: d; data: (camera proc image intrinsic matrix)";
119 
120  fs.release();
121  cout << "calibration ended" << endl;
122 
123  cam_sub.shutdown(); // the subscriber only runs one time
124 
125 }
126 
127 int main( int argc, char* argv[])
128 {
129  ros::init(argc, argv, "sensor_fusion");
130 
131  ros::NodeHandle nh("~");
132 
133  cam_sub = nh.subscribe("/xb3/right/new_info/image_rect_color", 1, cameraCalibration);
134  cam_info_sub = nh.subscribe("/xb3/right/new_info/camera_info", 1, cameraInfo);
135 
136  nh.param<std::string>("fileStorageYAML",fileStorageYAML,"");
137 
138  nh.param<std::string>("imagePoints1FileName",imagePoints1FileName,"");
139 
140  nh.param<std::string>("objectPoints1FileName",objectPoints1FileName,"");
141 
142  // Output arguments
143  std::cout << "fileStorageYAML: " << fileStorageYAML << std::endl;
144 
145  std::cout << "imagePointsFileName: " << imagePoints1FileName << std::endl;
146 
147  std::cout << "objectPointsFileName: " << objectPoints1FileName << std::endl;
148 
149  ros::spin();
150  return 0;
151 }
152 
153 
154 std::vector<cv::Point2f> Read2DPoints(const std::string& filename)
155 {
156  // Read points
157  std::ifstream pointsstream(filename.c_str());
158 
159  if(pointsstream == NULL)
160  {
161  std::cout << "Cannot open file " << filename << std::endl;
162  exit(-1);
163  }
164 
165  // Read the point from the first image
166  std::string line;
167  std::vector<cv::Point2f> points;
168 
169  while(getline(pointsstream, line))
170  {
171  std::stringstream ss(line);
172  float x,y;
173  ss >> x >> y;
174  points.push_back(cv::Point2f(x,y));
175  }
176 
177  return points;
178 }
179 
180 
181 std::vector<cv::Point3f> Read3DPoints(const std::string& filename)
182 {
183  // Read points
184  std::ifstream pointsstream(filename.c_str());
185 
186  if(pointsstream == NULL)
187  {
188  std::cout << "Cannot open file " << filename << std::endl;
189  exit(-1);
190  }
191 
192  // Read the point from the first image
193  std::string line;
194  std::vector<cv::Point3f> points;
195 
196  while(getline(pointsstream, line))
197  {
198  std::stringstream ss(line);
199  float x,y,z;
200  ss >> x >> y >> z;
201  points.push_back(cv::Point3f(x,y,z));
202  }
203 
204  return points;
205 }
std::string imagePoints1FileName
ros::Subscriber cam_info_sub
ros::Subscriber cam_sub
int main(int argc, char *argv[])
std::string objectPoints1FileName
void cameraInfo(const sensor_msgs::CameraInfo &cam_info_msg)
void cameraCalibration(const sensor_msgs::ImageConstPtr &cam_msg)
vector< double > k_matriz
std::vector< cv::Point2f > Read2DPoints(const std::string &filename)
std::vector< cv::Point3f > Read3DPoints(const std::string &filename)


multimodal_pedestrian_detect
Author(s): Rui Azevedo
autogenerated on Mon Mar 2 2015 01:32:27