Projector.cpp
Go to the documentation of this file.
00001 #include "pano_core/Projector.h"
00002 #include "pano_core/Camera.h"
00003 #include <cmath>
00004 #include <opencv2/core/core.hpp>
00005 #include <iostream>
00006 #include <vector>
00007 
00008 using namespace cv;
00009 
00010 namespace pano
00011 {
00012 
00013 static const float PI = 3.14159265f;
00014 /**** static functions *********/
00015 
00016 void Projector::getSphereRMap(const cv::Mat& K, const cv::Mat& R, cv::Mat& remap1, cv::Mat&remap2,
00017                               const cv::Mat& spherical_points, const cv::Size& output_size)
00018 {
00019   static std::vector<cv::Mat> working_mats;
00020   getSphereRMap(K, R, remap1, remap2, spherical_points, output_size, working_mats);
00021 
00022 }
00023 
00024 void Projector::getSphereGMap(const cv::Mat& K, const cv::Mat& G, cv::Mat& remap1, cv::Mat&remap2,
00025                               const cv::Mat& spherical_points, const cv::Size& output_size)
00026 {
00027   static std::vector<cv::Mat> working_mats;
00028   getSphereGMap(K, G, remap1, remap2, spherical_points, output_size, working_mats);
00029 }
00030 void Projector::getSphereRMapMask(const cv::Mat& _K, const cv::Mat& R, cv::Mat& map, cv::Mat& _mask,
00031                                   const cv::Mat& spherical_coords, cv::Mat& tm)
00032 {
00033 
00034   if (map.size() != spherical_coords.size())
00035   {
00036     map = Mat(spherical_coords.size(), cv::DataType<cv::Point2f>::type);
00037   }
00038 
00039   Mat& mask = _mask;
00040   mask = Mat_<unsigned char>::zeros(map.size());
00041 
00042   cv::Mat K;
00043   _K.convertTo(K, CV_32F);
00044   float fx, fy, cx, cy;
00045 
00046   fx = K.at<float> (0, 0);
00047   fy = K.at<float> (1, 1);
00048 
00049   cx = K.at<float> (0, 2);
00050   cy = K.at<float> (1, 2);
00051 
00052   float fovx, fovy;
00053   Camera::KtoFOV(K, fovx, fovy);
00054 
00055   Rect valid_rect(0, 0, 2 * cx, 2 * cy);
00056 
00057   // float cosfov = cos(KtoFOVmax(K));
00058 
00059   //rotate our unit sphere of points by TM.
00060   //so the imput image is "rotated" on the sphere.
00061   cv::transform(spherical_coords, tm, R);
00062 
00063   //this is the center pixel of image.
00064   // cv::Point3d z(0, 0, 1);
00065 
00066   //iterate over the entire output image
00067   //and calculate the mapping
00068   for (int y = 0; y < spherical_coords.rows; y++)
00069   {
00070     for (int x = 0; x < spherical_coords.cols; x++)
00071     {
00072 
00073       cv::Point3f & point = tm.at<cv::Point3f> (y, x);
00074 
00075       cv::Point2f & mappoint = map.at<cv::Point2f> (y, x);
00076 
00077       Point2f imgpt;
00078       imgpt.x = fx * point.x / point.z + cx;
00079       imgpt.y = fy * point.y / point.z + cy;
00080       mask.at<unsigned char> (y, x) = valid_rect.contains(Point(imgpt.x, imgpt.y)) && point.z > 0;
00081       //only map points that are valid pixel coords
00082       if (mask.at<unsigned char> (y, x))
00083       {
00084         //project the rotated sphere points into impute image coordinates
00085         //using the camera intrinsic parameters
00086         //so the center pixel of the input image is given by
00087         //a 3d vector of (0,0,1)
00088         //
00089         mappoint = imgpt;
00090       }
00091       else
00092       {
00093 
00094         int xs = point.x <= 0.01 ? -1 : 1;
00095         int ys = point.y <= 0.01 ? -1 : 1;
00096         mappoint.x = 1.0e4 * xs;
00097         mappoint.y = 1.0e4 * ys;
00098       }
00099 
00100     }
00101   }
00102 
00103 }
00104 void Projector::getSphereRMap(const cv::Mat& _K, const cv::Mat& R, cv::Mat& remap1, cv::Mat&remap2,
00105                               const cv::Mat&spherical_coords, const cv::Size& output_size,
00106                               std::vector<cv::Mat>& working_mats)
00107 {
00108   if (working_mats.size() != 4)
00109   {
00110     working_mats.clear();
00111     working_mats.resize(4);
00112   }
00113   Mat &tm = working_mats[0];
00114   Mat &map = working_mats[1];
00115   Mat &scaled = working_mats[2];
00116   Mat &mask = working_mats[3];
00117 
00118   getSphereRMapMask(_K, R, map, mask, spherical_coords, tm);
00119   float _scaling_matrix[] = {output_size.width / (float)map.size().width, 0, 0, 0, output_size.height
00120       / (float)map.size().height, 0};
00121   warpAffine(map, scaled, Mat(2, 3, CV_32F, _scaling_matrix), output_size, INTER_LINEAR, BORDER_WRAP);
00122   convertMaps(scaled, Mat(), remap1, remap2, CV_16SC2);
00123 }
00124 
00125 void Projector::getSphereGMap(const cv::Mat& _K, const cv::Mat& G, cv::Mat& remap1, cv::Mat&remap2,
00126                               const cv::Mat & homog_sphr_coords, const cv::Size& output_size,
00127                               std::vector<cv::Mat>& working_mats)
00128 {
00129   if (working_mats.size() != 3)
00130   {
00131     working_mats.clear();
00132     working_mats.resize(3);
00133   }
00134   Mat &tm = working_mats[0];
00135   Mat &map = working_mats[1];
00136   if (map.size() != homog_sphr_coords.size())
00137   {
00138     map = Mat(homog_sphr_coords.size(), cv::DataType<cv::Point2f>::type);
00139   }
00140   if (G.cols != 4 || G.rows != 4)
00141   {
00142     throw cv::Exception(1, "Invalid G! Must be 4x4!", "Projector.cpp", "", 0);
00143   }
00144   map = Scalar(INFINITY, INFINITY, INFINITY, INFINITY);
00145 
00146   Mat &scaled = working_mats[2];
00147   cv::Mat K;
00148   _K.convertTo(K, CV_32F);
00149   float fx, fy, cx, cy;
00150 
00151   fx = K.at<float> (0, 0);
00152   fy = K.at<float> (1, 1);
00153   cx = K.at<float> (0, 2);
00154   cy = K.at<float> (1, 2);
00155 
00156   cv::transform(homog_sphr_coords, tm, G);
00157 
00158   float lambda;
00159   for (int y = 0; y < homog_sphr_coords.rows; y++)
00160   {
00161     for (int x = 0; x < homog_sphr_coords.cols; x++)
00162     {
00163       cv::Vec4f & point = tm.at<cv::Vec4f> (y, x);
00164       cv::Point2f & mappoint = map.at<cv::Point2f> (y, x);
00165       lambda = 1e-1;
00166       if (point[2] < lambda)
00167       {
00168         int xs = point[0] <= 0.01 ? -1 : 1;
00169         int ys = point[1] <= 0.01 ? -1 : 1;
00170         mappoint.x = 1.0e4 * xs;
00171         mappoint.y = 1.0e4 * ys;
00172         continue;
00173       }
00174 
00175       mappoint.x = fx * point[0] / point[2] + cx;
00176       mappoint.y = fy * point[1] / point[2] + cy;
00177 
00178     }
00179   }
00180 
00181   float _scaling_matrix[] = {output_size.width / (float)map.size().width, 0, 0, 0, output_size.height
00182       / (float)map.size().height, 0};
00183   warpAffine(map, scaled, Mat(2, 3, CV_32F, _scaling_matrix), output_size, INTER_LINEAR, BORDER_WRAP);
00184 
00185   convertMaps(scaled, Mat(), remap1, remap2, CV_16SC2);
00186 }
00187 
00188 cv::Mat createHomogSphrCoords(const cv::Size& sphere_size, float theta_range = 2 * CV_PI, float phi_range = CV_PI)
00189 {
00190 
00191   Mat _homog_sphr_coords = Mat(sphere_size, cv::DataType<cv::Vec4f>::type);
00192   for (int i = 0; i < _homog_sphr_coords.rows; i++)
00193   {
00194 
00195     float phi = (i - _homog_sphr_coords.rows / 2.0f) * (phi_range) / _homog_sphr_coords.rows;
00196     float sinphi = std::sin(phi);
00197     float cosphi = std::cos(phi);
00198     for (int j = 0; j < _homog_sphr_coords.cols; j++)
00199     {
00200 
00201       float theta = (j - _homog_sphr_coords.cols / 2.0f) * (theta_range) / _homog_sphr_coords.cols;
00202       float sintheta = std::sin(theta);
00203       float costheta = std::cos(theta);
00204 
00205       cv::Point3f point = cv::Point3f(sintheta * cosphi, sinphi, costheta * cosphi);
00206 
00207       cv::Vec4f homog_pt(point.x, point.y, point.z, 1.0);
00208       _homog_sphr_coords.at<cv::Vec4f> (i, j) = homog_pt;
00209 
00210     }
00211   }
00212   return _homog_sphr_coords;
00213 
00214 }
00215 
00216 cv::Mat Projector::createSphericalCoords(const cv::Size& sphere_size, float theta_range, float phi_range)
00217 {
00218 
00219   Mat sc;
00220   createSphericalCoords(sphere_size, -theta_range / 2, theta_range / 2, -phi_range / 2, phi_range / 2, sc);
00221   return sc;
00222 }
00223 
00224 void Projector::createSphericalCoords(const cv::Size& sphere_size, float theta_0, float theta_1, float phi_0,
00225                                       float phi_1, Mat& spherical_coords)
00226 {
00227 
00228   spherical_coords.create(sphere_size, cv::DataType<cv::Point3f>::type);
00229   //this creates a matrix of 3d points for the entire output image, all of these points lie on
00230   //a unit sphere.
00231   // [theta, phi] = meshgrid( linspace(-pi,pi,1000),linspace(-pi/2,pi/2,500) );
00232   // rho          = 1 + 0*theta;
00233   // [x,y,z]      = sphr2cart( theta(:),phi(:),rho(:));
00234   // ** Subtle Side Effect ** : 
00235   // theta=0 is meant to be north, but corresponds to x-axis
00236   //      or "East" when drawing a polar diagram. 
00237 
00238   float phi_range   = (phi_1 - phi_0);
00239   float theta_range = (theta_1 - theta_0);
00240   float phi_step    = (phi_range) / spherical_coords.rows;
00241 
00242 
00243   float theta_step = (theta_range) / spherical_coords.cols;
00244   for (int y = 0; y < spherical_coords.rows; y++)
00245   {
00246 
00247     float phi = phi_0 + y * phi_step;
00248 
00249     float sinphi  = std::sin(phi);
00250 
00251     float cosphi = std::cos(phi);
00252     for (int x = 0; x < spherical_coords.cols; x++)
00253     {
00254       float theta = theta_0 + x * theta_step;
00255       float sintheta = std::sin(theta);
00256       float costheta = std::cos(theta);
00257 
00258       cv::Point3f point = cv::Point3f(sintheta * cosphi, sinphi, costheta * cosphi);
00259       spherical_coords.at<cv::Point3f> (y, x) = point;
00260     }
00261   }
00262 }
00263 
00264 void Projector::getSphereMask(const cv::Mat& onezies, const cv::Mat& remap1, const cv::Mat&remap2, cv::Mat& mask)
00265 {
00266   if (mask.size() != remap1.size() || mask.type() != onezies.type())
00267   {
00268 
00269     mask = Mat::zeros(remap1.size(), onezies.type());
00270   }
00271   cv::remap(onezies, mask, remap1, remap2, cv::INTER_AREA, cv::BORDER_TRANSPARENT);
00272 }
00273 void Projector::projectImage(const cv::Mat& image, const cv::Mat& remap1, const cv::Mat&remap2, cv::Mat & outputimage,
00274                              int filltype, const Scalar& value)
00275 {
00276   if (outputimage.size() != remap1.size() || outputimage.type() != image.type())
00277   {
00278 
00279     outputimage = Mat::zeros(remap1.size(), image.type());
00280   }
00281 
00282   int interp_mode = cv::INTER_LINEAR; // cv::INTER_AREA; Better ?!
00283 
00284   cv::remap(image, outputimage, remap1, remap2, interp_mode, filltype, value);
00285 
00286 }
00287 
00288 Projector::Projector(const cv::Size& output_size, float theta_range, float phi_range) :
00289   outimage_size(output_size), spherical_coords(createSphericalCoords(Size(output_size.width / 10.0f, output_size.height
00290       / 10.0f), theta_range, phi_range))
00291 {
00292 
00293 }
00294 
00295 Projector::~Projector(void)
00296 {
00297 }
00298 
00299 namespace
00300 {
00301 bool checksize(const cv::Size& lhs, const cv::Size& rhs)
00302 {
00303   return lhs.width == rhs.width && lhs.height == rhs.height;
00304 }
00305 }
00306 
00310 void Projector::setSRandK(const cv::Size& inputsz, const cv::Mat& R, const cv::Mat& _K)
00311 {
00312   input_image_sz = inputsz;
00313   K = _K.clone();
00314   getSphereRMap(K, R, remap1, remap2, spherical_coords, outimage_size, working_mats);
00315 
00316 }
00317 
00318 void Projector::projectMat(const cv::Mat& image, cv::Mat& outimage, int filltype, const Scalar& value)
00319 {
00320 
00321   projectImage(image, remap1, remap2, outimage, filltype, value);
00322 
00323 }
00324 namespace
00325 {
00326 std::vector<cv::Rect> GenRois(cv::Size output_size, cv::Size grids)
00327 {
00328   std::vector<cv::Rect> rois(grids.area());
00329   float width = output_size.width / float(grids.width);
00330   float height = output_size.height / float(grids.height);
00331   for (int x = 0; x < grids.width; x++)
00332   {
00333     for (int y = 0; y < grids.height; y++)
00334     {
00335       Point2f imgpt(x * width, y * height);
00336       Rect& roi = rois[x + y * grids.width];
00337       roi = cv::Rect(imgpt.x, imgpt.y, width, height);
00338     }
00339   }
00340   return rois;
00341 }
00342 }
00343 SparseProjector::SparseProjector(const cv::Size& output_size, const cv::Size& n_grids) :
00344   output_size_(output_size),spherical_coords_small_(Projector::createSphericalCoords(Size(100,50),2 * CV_PI,CV_PI)), grids_(n_grids), rois_(GenRois(output_size_, grids_)),
00345       small_rois_(GenRois(spherical_coords_small_.size(), grids_)), workingid_(0)
00346 {
00347 
00348 }
00349 
00353 void SparseProjector::setSRandK(const cv::Size& inputsz, const cv::Mat& R, const cv::Mat& K, std::vector<int>& roi_ids)
00354 {
00355   Projector::getSphereRMapMask(K, R, remap_, mask_, spherical_coords_small_, tm_);
00356   roi_ids.clear();
00357   for (int i = 0; i < int(rois_.size()); i++)
00358   {
00359     const Rect& roi = small_rois_[i];
00360     Mat m = mask_(roi);
00361     if (countNonZero(m))
00362     {
00363       roi_ids.push_back(i);
00364     }
00365   }
00366 
00367   K_ = K;
00368   R_ = R;
00369   workingid_ = -1;
00370 }
00371 
00372 void SparseProjector::setWorkingRoi(int id)
00373 {
00374   if (workingid_ == id)
00375     return;
00376   workingid_ = id;
00377 
00378   Rect roi = small_rois_[id];
00379   Rect broi = rois_[id];
00380   float theta_0, theta_1, phi_0, phi_1;
00381   float theta_step = (2 * CV_PI) / output_size_.width;
00382   float phi_step = CV_PI / output_size_.height;
00383 
00384   theta_0 = (broi.x - output_size_.width / float(2)) * theta_step;
00385   theta_1 = theta_0 + broi.width * theta_step;
00386 
00387   phi_0 = (broi.y - output_size_.height / float(2)) * phi_step;// - broi.height * phi_step/2;
00388   phi_1 = phi_0 + broi.height * phi_step;
00389 
00390   Projector::createSphericalCoords(broi.size(), theta_0, theta_1, phi_0, phi_1, spherical_coords_);
00391 
00392   Mat map, mask, tm;
00393   Projector::getSphereRMapMask(K_, R_, map, mask, spherical_coords_, tm);
00394   convertMaps(map, Mat(), remap1_, remap2_, CV_16SC2);
00395 }
00396 
00397 void SparseProjector::projectMat(int roi_id, const cv::Mat& m, cv::Mat& outimage, int filltype, const cv::Scalar& value)
00398 {
00399   setWorkingRoi(roi_id);
00400   Projector::projectImage(m, remap1_, remap2_, outimage, filltype, value);
00401 }
00402 
00403 }


pano_core
Author(s): Ethan Rublee
autogenerated on Wed Aug 26 2015 16:34:01