Camera.cpp
Go to the documentation of this file.
00001 /*
00002  * Camera.cpp
00003  *
00004  *  Created on: Oct 17, 2010
00005  *      Author: ethan
00006  */
00007 
00008 #include "pano_core/Camera.h"
00009 #include <pano_core/ModelFitter.h>
00010 
00011 #include <opencv2/core/core.hpp>
00012 #include <opencv2/highgui/highgui.hpp>
00013 #include <opencv2/calib3d/calib3d.hpp>
00014 #include <opencv2/imgproc/imgproc.hpp>
00015 
00016 #include <iostream>
00017 
00018 using namespace cv;
00019 
00020 namespace pano
00021 {
00022 namespace
00023 {
00024 
00025 bool readKfromCalib(cv::Mat& K, cv::Mat& distortion, cv::Size & img_size, const std::string& calibfile)
00026 {
00027   cv::FileStorage fs(calibfile, cv::FileStorage::READ);
00028   cv::Mat cameramat;
00029   cv::Mat cameradistortion;
00030   float width = 0, height = 0;
00031   if (fs.isOpened())
00032   {
00033     cv::read(fs["camera_matrix"], cameramat, cv::Mat());
00034     cv::read(fs["distortion_coefficients"], cameradistortion, cv::Mat());
00035     cv::read(fs["image_width"], width, 0);
00036     cv::read(fs["image_height"], height, 0);
00037 
00038     fs.release();
00039 
00040   }
00041   else
00042   {
00043     return false;
00044   }
00045 
00046   cv::Size _size(width, height);
00047   img_size = _size;
00048 
00049   K = cameramat;
00050 
00051   distortion = cameradistortion;
00052   return true;
00053 }
00054 
00055 }
00056 
00057 
00058 Camera::Camera() :
00059   K_(Mat::eye(3, 3, CV_32F))
00060 {
00061 
00062 }
00063 
00064 Camera::Camera(const std::string& camera_file) :
00065   K_(Mat::eye(3, 3, CV_32F))
00066 {
00067   setCameraIntrinsics(camera_file);
00068 }
00069 Camera::~Camera()
00070 {
00071 
00072 }
00073 void Camera::initUndistort(){
00074   if(P_.empty() && !D_.empty()){
00075     P_ = getOptimalNewCameraMatrix(K_, D_, img_size_, 0);
00076     initUndistortRectifyMap(K_,D_,Mat(),P_,img_size_,CV_16SC2,u_rm1_,u_rm2_);
00077   }
00078 }
00079 
00080 void Camera::undistort(const cv::Mat&image ,cv::Mat& uimage) const{
00081  if(!P_.empty()){
00082   cv::remap(image,uimage,u_rm1_,u_rm2_,CV_INTER_LINEAR);
00083  }else
00084    uimage = image;
00085 }
00086 void Camera::ptsToRays(const std::vector<cv::Point2f>& pts, std::vector<cv::Point3f>& rays) const
00087 {
00088   rays.resize(pts.size());
00089   points2fto3f(pts.begin(), pts.end(), rays.begin(), Pinv_.empty() ? Kinv_ : Pinv_);
00090  // proje
00091 }
00092 void Camera::raysToPts(const std::vector<cv::Point3f>& rays, std::vector<cv::Point2f>& pts) const
00093 {
00094   pts.resize(rays.size());
00095   points3fto2f(rays.begin(), rays.end(), pts.begin(), P_.empty() ? K_ : P_);
00096   //projectPoints()
00097 }
00098 namespace
00099 {
00100 void force2float(cv::Mat & m)
00101 {
00102   if (m.empty())
00103     return;
00104   Mat t;
00105   m.convertTo(t, CV_32FC1);
00106   m = t;
00107 }
00108 }
00109 void Camera::setupK()
00110 {
00111   force2float(K_);
00112   force2float(D_);
00113 
00114   Camera::KtoFOV(K_, fov_x_, fov_y_);
00115   Kinv_ = K_.inv();
00116 }
00117 void Camera::setCameraIntrinsics(const std::string& filename)
00118 {
00119   if (!readKfromCalib(K_, D_, img_size_, filename))
00120   {
00121     std::cerr << "Bad read on the Calibration File! : " << filename << std::endl;
00122     throw std::runtime_error("bad calibration file : " + filename);
00123   }
00124   setupK();
00125 }
00126 void Camera::setCameraIntrinsics(const cv::Mat& K, const cv::Mat& D, const cv::Size& img_size)
00127 {
00128   K_ = K;
00129   D_ = D;
00130 
00131   img_size_ = img_size;
00132 
00133   setupK();
00134 }
00135 
00136 void Camera::scale(float x,float y){
00137   K_ = K_.clone();
00138   K_.at<float>(0,0) *=x;
00139   K_.at<float>(1,1) *=y;
00140   K_.at<float>(0,2) *=x;
00141   K_.at<float>(1,2) *=y;
00142   img_size_.width *= x;
00143   img_size_.height *=y;
00144   setupK();
00145 }
00146 
00147 
00148 
00149 void Camera::write(const std::string& file_name) const{
00150         FileStorage fs(file_name,FileStorage::WRITE);
00151         fs << "camera";
00152         serialize(fs);
00153 }
00154 void Camera::serialize(cv::FileStorage& fs) const
00155 {
00156   fs << "{";
00157   cvWriteComment(*fs,"Camera class" , 0);
00158   fs << "K" << K_;
00159   fs << "Kinv" << Kinv_;
00160   if(!D_.empty())
00161     fs << "D" << D_;
00162   fs << "width" << img_size_.width;
00163   fs << "height" << img_size_.height;
00164   fs << "}";
00165 }
00166 void Camera::deserialize(const cv::FileNode& fn)
00167 {
00168   fn["K"] >> K_;
00169   fn["Kinv"] >> Kinv_;
00170   if(!fn["D"].empty())
00171     fn["D"] >> D_;
00172   img_size_.width = (int)fn["width"] ;
00173   img_size_.height = (int)fn["height"] ;
00174   setupK();
00175 }
00176 
00177 void Camera::KtoFOV(const cv::Mat& K, float & fovx, float & fovy)
00178   {
00179     int K_type = K.type();
00180     CV_Assert( K_type == CV_32FC1 || K_type == CV_64FC1 )
00181       ;
00182 
00183     switch (K_type)
00184     {
00185       case CV_32FC1:
00186         fovx = 2.0f * std::atan(K.at<float> (0, 2) / K.at<float> (0, 0));
00187         fovy = 2.0f * std::atan(K.at<float> (1, 2) / K.at<float> (1, 1));
00188         break;
00189       case CV_64FC1:
00190         fovx = 2.0f * std::atan(K.at<double> (0, 2) / K.at<double> (0, 0));
00191         fovy = 2.0f * std::atan(K.at<double> (1, 2) / K.at<double> (1, 1));
00192         break;
00193       default:
00194         break;
00195     }
00196   }
00197 
00198 }


pano_core
Author(s): Ethan Rublee
autogenerated on Mon Mar 14 2016 10:56:54