3 #include <camera_info_manager/camera_info_manager.h>
5 #include "opencv2/core/core.hpp"
6 #include "opencv2/imgproc/imgproc.hpp"
7 #include "opencv2/calib3d/calib3d.hpp"
8 #include "opencv2/highgui/highgui.hpp"
14 #include "mtt/TargetList.h"
20 #include <image_transport/image_transport.h>
22 #include <cv_bridge/cv_bridge.h>
23 #include <opencv/cvwimage.h>
25 #include <visualization_msgs/Marker.h>
26 #include <visualization_msgs/MarkerArray.h>
28 #include "sensor_msgs/LaserScan.h"
29 #include <sensor_msgs/image_encodings.h>
31 #include <boost/thread.hpp>
32 #include <boost/shared_ptr.hpp>
44 std::vector<cv::Point2f>
Read2DPoints(
const std::string& filename);
45 std::vector<cv::Point3f>
Read3DPoints(
const std::string& filename);
52 void cameraInfo(
const sensor_msgs::CameraInfo& cam_info_msg)
55 for (
int i=0; i<cam_info_msg.K.size(); i++)
57 k = cam_info_msg.K[i];
66 cv_bridge::CvImagePtr cv_ptr;
70 cv_ptr = cv_bridge::toCvCopy(cam_msg, sensor_msgs::image_encodings::BGR8);
72 catch (cv_bridge::Exception& e)
74 ROS_ERROR(
"cv_bridge exception: %s", e.what());
83 cv::Mat initialCameraMatrix = cvCreateMat(3,3 , CV_64FC1);
84 for (
int i = 0; i < 3; i++)
86 for (
int j = 0; j < 3; j++)
88 initialCameraMatrix.at<
double>(i, j) =
k_matriz[3*i+j];
92 cv::Mat distCoeffs = cvCreateMat(1,5 , CV_64FC1);
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;
100 cv::Mat cameraMatrix;
104 cameraMatrix = initialCameraMatrix;
106 cv::solvePnP(inputObject1Points, inputImage1Points, cameraMatrix, distCoeffs, rvecs, tvecs);
109 Rodrigues(rvecs, rvecs_3x3);
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));
116 Mat T = (Mat_<double>(1,3) << tvecs.at<
double>(0,0), tvecs.at<
double>(0,1), tvecs.at<
double>(0,2));
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)";
121 cout <<
"calibration ended" << endl;
127 int main(
int argc,
char* argv[])
129 ros::init(argc, argv,
"sensor_fusion");
131 ros::NodeHandle nh(
"~");
143 std::cout <<
"fileStorageYAML: " << fileStorageYAML << std::endl;
145 std::cout <<
"imagePointsFileName: " << imagePoints1FileName << std::endl;
147 std::cout <<
"objectPointsFileName: " << objectPoints1FileName << std::endl;
157 std::ifstream pointsstream(filename.c_str());
159 if(pointsstream == NULL)
161 std::cout <<
"Cannot open file " << filename << std::endl;
167 std::vector<cv::Point2f> points;
169 while(getline(pointsstream, line))
171 std::stringstream ss(line);
174 points.push_back(cv::Point2f(x,y));
184 std::ifstream pointsstream(filename.c_str());
186 if(pointsstream == NULL)
188 std::cout <<
"Cannot open file " << filename << std::endl;
194 std::vector<cv::Point3f> points;
196 while(getline(pointsstream, line))
198 std::stringstream ss(line);
201 points.push_back(cv::Point3f(x,y,z));
std::string imagePoints1FileName
ros::Subscriber cam_info_sub
int main(int argc, char *argv[])
std::string objectPoints1FileName
std::string fileStorageYAML
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)