modules/stitching/src/warpers.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                          License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.\r
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.\r
  15 // Third party copyrights are property of their respective owners.\r
  16 //\r
  17 // Redistribution and use in source and binary forms, with or without modification,\r
  18 // are permitted provided that the following conditions are met:\r
  19 //\r
  20 //   * Redistribution's of source code must retain the above copyright notice,\r
  21 //     this list of conditions and the following disclaimer.\r
  22 //\r
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  24 //     this list of conditions and the following disclaimer in the documentation\r
  25 //     and/or other materials provided with the distribution.\r
  26 //\r
  27 //   * The name of the copyright holders may not be used to endorse or promote products\r
  28 //     derived from this software without specific prior written permission.\r
  29 //\r
  30 // This software is provided by the copyright holders and contributors "as is" and\r
  31 // any express or implied warranties, including, but not limited to, the implied\r
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  34 // indirect, incidental, special, exemplary, or consequential damages\r
  35 // (including, but not limited to, procurement of substitute goods or services;\r
  36 // loss of use, data, or profits; or business interruption) however caused\r
  37 // and on any theory of liability, whether in contract, strict liability,\r
  38 // or tort (including negligence or otherwise) arising in any way out of\r
  39 // the use of this software, even if advised of the possibility of such damage.\r
  40 //\r
  41 //M*/\r
  42 \r
  43 #include "precomp.hpp"\r
  44 \r
  45 using namespace std;\r
  46 \r
  47 namespace cv {\r
  48 namespace detail {\r
  49 \r
  50 void ProjectorBase::setCameraParams(const Mat &K, const Mat &R, const Mat &T)\r
  51 {\r
  52     CV_Assert(K.size() == Size(3, 3) && K.type() == CV_32F);\r
  53     CV_Assert(R.size() == Size(3, 3) && R.type() == CV_32F);\r
  54     CV_Assert((T.size() == Size(1, 3) || T.size() == Size(3, 1)) && T.type() == CV_32F);\r
  55 \r
  56     Mat_<float> K_(K);\r
  57     k[0] = K_(0,0); k[1] = K_(0,1); k[2] = K_(0,2);\r
  58     k[3] = K_(1,0); k[4] = K_(1,1); k[5] = K_(1,2);\r
  59     k[6] = K_(2,0); k[7] = K_(2,1); k[8] = K_(2,2);\r
  60 \r
  61     Mat_<float> Rinv = R.t();\r
  62     rinv[0] = Rinv(0,0); rinv[1] = Rinv(0,1); rinv[2] = Rinv(0,2);\r
  63     rinv[3] = Rinv(1,0); rinv[4] = Rinv(1,1); rinv[5] = Rinv(1,2);\r
  64     rinv[6] = Rinv(2,0); rinv[7] = Rinv(2,1); rinv[8] = Rinv(2,2);\r
  65 \r
  66     Mat_<float> R_Kinv = R * K.inv();\r
  67     r_kinv[0] = R_Kinv(0,0); r_kinv[1] = R_Kinv(0,1); r_kinv[2] = R_Kinv(0,2); \r
  68     r_kinv[3] = R_Kinv(1,0); r_kinv[4] = R_Kinv(1,1); r_kinv[5] = R_Kinv(1,2); \r
  69     r_kinv[6] = R_Kinv(2,0); r_kinv[7] = R_Kinv(2,1); r_kinv[8] = R_Kinv(2,2); \r
  70 \r
  71     Mat_<float> K_Rinv = K * Rinv;\r
  72     k_rinv[0] = K_Rinv(0,0); k_rinv[1] = K_Rinv(0,1); k_rinv[2] = K_Rinv(0,2); \r
  73     k_rinv[3] = K_Rinv(1,0); k_rinv[4] = K_Rinv(1,1); k_rinv[5] = K_Rinv(1,2); \r
  74     k_rinv[6] = K_Rinv(2,0); k_rinv[7] = K_Rinv(2,1); k_rinv[8] = K_Rinv(2,2); \r
  75 \r
  76     Mat_<float> T_(T.reshape(0, 3));\r
  77     t[0] = T_(0,0); t[1] = T_(1,0); t[2] = T_(2,0);\r
  78 }\r
  79 \r
  80 \r
  81 Rect PlaneWarper::buildMaps(Size src_size, const Mat &K, const Mat &R, const Mat &T, Mat &xmap, Mat &ymap)\r
  82 {\r
  83     projector_.setCameraParams(K, R, T);\r
  84 \r
  85     Point dst_tl, dst_br;\r
  86     detectResultRoi(src_size, dst_tl, dst_br);\r
  87 \r
  88     xmap.create(dst_br.y - dst_tl.y + 1, dst_br.x - dst_tl.x + 1, CV_32F);\r
  89     ymap.create(dst_br.y - dst_tl.y + 1, dst_br.x - dst_tl.x + 1, CV_32F);\r
  90 \r
  91     float x, y;\r
  92     for (int v = dst_tl.y; v <= dst_br.y; ++v)\r
  93     {\r
  94         for (int u = dst_tl.x; u <= dst_br.x; ++u)\r
  95         {\r
  96             projector_.mapBackward(static_cast<float>(u), static_cast<float>(v), x, y);\r
  97             xmap.at<float>(v - dst_tl.y, u - dst_tl.x) = x;\r
  98             ymap.at<float>(v - dst_tl.y, u - dst_tl.x) = y;\r
  99         }\r
 100     }\r
 101 \r
 102     return Rect(dst_tl, dst_br);\r
 103 }\r
 104 \r
 105 \r
 106 Point PlaneWarper::warp(const Mat &src, const Mat &K, const Mat &R, const Mat &T, int interp_mode, int border_mode,\r
 107                         Mat &dst)\r
 108 {\r
 109     Mat xmap, ymap;\r
 110     Rect dst_roi = buildMaps(src.size(), K, R, T, xmap, ymap);\r
 111 \r
 112     dst.create(dst_roi.height + 1, dst_roi.width + 1, src.type());\r
 113     remap(src, dst, xmap, ymap, interp_mode, border_mode);\r
 114 \r
 115     return dst_roi.tl();\r
 116 }\r
 117 \r
 118 \r
 119 Rect PlaneWarper::warpRoi(Size src_size, const Mat &K, const Mat &R, const Mat &T)\r
 120 {\r
 121     projector_.setCameraParams(K, R, T);\r
 122 \r
 123     Point dst_tl, dst_br;\r
 124     detectResultRoi(src_size, dst_tl, dst_br);\r
 125 \r
 126     return Rect(dst_tl, Point(dst_br.x + 1, dst_br.y + 1));\r
 127 }\r
 128 \r
 129 \r
 130 void PlaneWarper::detectResultRoi(Size src_size, Point &dst_tl, Point &dst_br)\r
 131 {\r
 132     float tl_uf = numeric_limits<float>::max();\r
 133     float tl_vf = numeric_limits<float>::max();\r
 134     float br_uf = -numeric_limits<float>::max();\r
 135     float br_vf = -numeric_limits<float>::max();\r
 136 \r
 137     float u, v;\r
 138 \r
 139     projector_.mapForward(0, 0, u, v);\r
 140     tl_uf = min(tl_uf, u); tl_vf = min(tl_vf, v);\r
 141     br_uf = max(br_uf, u); br_vf = max(br_vf, v);\r
 142 \r
 143     projector_.mapForward(0, static_cast<float>(src_size.height - 1), u, v);\r
 144     tl_uf = min(tl_uf, u); tl_vf = min(tl_vf, v);\r
 145     br_uf = max(br_uf, u); br_vf = max(br_vf, v);\r
 146 \r
 147     projector_.mapForward(static_cast<float>(src_size.width - 1), 0, u, v);\r
 148     tl_uf = min(tl_uf, u); tl_vf = min(tl_vf, v);\r
 149     br_uf = max(br_uf, u); br_vf = max(br_vf, v);\r
 150 \r
 151     projector_.mapForward(static_cast<float>(src_size.width - 1), static_cast<float>(src_size.height - 1), u, v);\r
 152     tl_uf = min(tl_uf, u); tl_vf = min(tl_vf, v);\r
 153     br_uf = max(br_uf, u); br_vf = max(br_vf, v);\r
 154 \r
 155     dst_tl.x = static_cast<int>(tl_uf);\r
 156     dst_tl.y = static_cast<int>(tl_vf);\r
 157     dst_br.x = static_cast<int>(br_uf);\r
 158     dst_br.y = static_cast<int>(br_vf);\r
 159 }\r
 160 \r
 161 \r
 162 void SphericalWarper::detectResultRoi(Size src_size, Point &dst_tl, Point &dst_br)\r
 163 {\r
 164     detectResultRoiByBorder(src_size, dst_tl, dst_br);\r
 165 \r
 166     float tl_uf = static_cast<float>(dst_tl.x);\r
 167     float tl_vf = static_cast<float>(dst_tl.y);\r
 168     float br_uf = static_cast<float>(dst_br.x);\r
 169     float br_vf = static_cast<float>(dst_br.y);\r
 170 \r
 171     float x = projector_.rinv[1];\r
 172     float y = projector_.rinv[4];\r
 173     float z = projector_.rinv[7];\r
 174     if (y > 0.f)\r
 175     {\r
 176         float x_ = (projector_.k[0] * x + projector_.k[1] * y) / z + projector_.k[2];\r
 177         float y_ = projector_.k[4] * y / z + projector_.k[5];\r
 178         if (x_ > 0.f && x_ < src_size.width && y_ > 0.f && y_ < src_size.height)\r
 179         {\r
 180             tl_uf = min(tl_uf, 0.f); tl_vf = min(tl_vf, static_cast<float>(CV_PI * projector_.scale));\r
 181             br_uf = max(br_uf, 0.f); br_vf = max(br_vf, static_cast<float>(CV_PI * projector_.scale));\r
 182         }\r
 183     }\r
 184 \r
 185     x = projector_.rinv[1];\r
 186     y = -projector_.rinv[4];\r
 187     z = projector_.rinv[7];\r
 188     if (y > 0.f)\r
 189     {\r
 190         float x_ = (projector_.k[0] * x + projector_.k[1] * y) / z + projector_.k[2];\r
 191         float y_ = projector_.k[4] * y / z + projector_.k[5];\r
 192         if (x_ > 0.f && x_ < src_size.width && y_ > 0.f && y_ < src_size.height)\r
 193         {\r
 194             tl_uf = min(tl_uf, 0.f); tl_vf = min(tl_vf, static_cast<float>(0));\r
 195             br_uf = max(br_uf, 0.f); br_vf = max(br_vf, static_cast<float>(0));\r
 196         }\r
 197     }\r
 198 \r
 199     dst_tl.x = static_cast<int>(tl_uf);\r
 200     dst_tl.y = static_cast<int>(tl_vf);\r
 201     dst_br.x = static_cast<int>(br_uf);\r
 202     dst_br.y = static_cast<int>(br_vf);\r
 203 }\r
 204 \r
 205 \r
 206 #ifndef ANDROID\r
 207 Rect PlaneWarperGpu::buildMaps(Size src_size, const Mat &K, const Mat &R, gpu::GpuMat &xmap, gpu::GpuMat &ymap)\r
 208 {\r
 209     return buildMaps(src_size, K, R, Mat::zeros(3, 1, CV_32F), xmap, ymap);\r
 210 }\r
 211 \r
 212 Rect PlaneWarperGpu::buildMaps(Size src_size, const Mat &K, const Mat &R, const Mat &T, gpu::GpuMat &xmap, gpu::GpuMat &ymap)\r
 213 {\r
 214     projector_.setCameraParams(K, R, T);\r
 215 \r
 216     Point dst_tl, dst_br;\r
 217     detectResultRoi(src_size, dst_tl, dst_br);\r
 218 \r
 219     gpu::buildWarpPlaneMaps(src_size, Rect(dst_tl, Point(dst_br.x+1, dst_br.y+1)),\r
 220                             K, R, projector_.scale, xmap, ymap);\r
 221 \r
 222     return Rect(dst_tl, dst_br);\r
 223 }\r
 224 \r
 225 Point PlaneWarperGpu::warp(const gpu::GpuMat &src, const Mat &K, const Mat &R, int interp_mode, int border_mode,\r
 226                            gpu::GpuMat &dst)\r
 227 {\r
 228     return warp(src, K, R, Mat::zeros(3, 1, CV_32F), interp_mode, border_mode, dst);\r
 229 }\r
 230 \r
 231 \r
 232 Point PlaneWarperGpu::warp(const gpu::GpuMat &src, const Mat &K, const Mat &R, const Mat &T, int interp_mode, int border_mode,\r
 233                            gpu::GpuMat &dst)\r
 234 {\r
 235     Rect dst_roi = buildMaps(src.size(), K, R, T, d_xmap_, d_ymap_);\r
 236     dst.create(dst_roi.height + 1, dst_roi.width + 1, src.type());\r
 237     gpu::remap(src, dst, d_xmap_, d_ymap_, interp_mode, border_mode);\r
 238     return dst_roi.tl();\r
 239 }\r
 240 \r
 241 \r
 242 Rect SphericalWarperGpu::buildMaps(Size src_size, const Mat &K, const Mat &R, gpu::GpuMat &xmap, gpu::GpuMat &ymap)\r
 243 {\r
 244     projector_.setCameraParams(K, R);\r
 245 \r
 246     Point dst_tl, dst_br;\r
 247     detectResultRoi(src_size, dst_tl, dst_br);\r
 248 \r
 249     gpu::buildWarpSphericalMaps(src_size, Rect(dst_tl, Point(dst_br.x + 1, dst_br.y + 1)),\r
 250                                 K, R, projector_.scale, xmap, ymap);\r
 251 \r
 252     return Rect(dst_tl, dst_br);\r
 253 }\r
 254 \r
 255 \r
 256 Point SphericalWarperGpu::warp(const gpu::GpuMat &src, const Mat &K, const Mat &R, int interp_mode, int border_mode,\r
 257                                gpu::GpuMat &dst)\r
 258 {\r
 259     Rect dst_roi = buildMaps(src.size(), K, R, d_xmap_, d_ymap_);\r
 260     dst.create(dst_roi.height + 1, dst_roi.width + 1, src.type());\r
 261     gpu::remap(src, dst, d_xmap_, d_ymap_, interp_mode, border_mode);\r
 262     return dst_roi.tl();\r
 263 }\r
 264 \r
 265 \r
 266 Rect CylindricalWarperGpu::buildMaps(Size src_size, const Mat &K, const Mat &R, gpu::GpuMat &xmap, gpu::GpuMat &ymap)\r
 267 {\r
 268     projector_.setCameraParams(K, R);\r
 269 \r
 270     Point dst_tl, dst_br;\r
 271     detectResultRoi(src_size, dst_tl, dst_br);\r
 272 \r
 273     gpu::buildWarpCylindricalMaps(src_size, Rect(dst_tl, Point(dst_br.x + 1, dst_br.y + 1)),\r
 274                                   K, R, projector_.scale, xmap, ymap);\r
 275 \r
 276     return Rect(dst_tl, dst_br);\r
 277 }\r
 278 \r
 279 \r
 280 Point CylindricalWarperGpu::warp(const gpu::GpuMat &src, const Mat &K, const Mat &R, int interp_mode, int border_mode,\r
 281                                  gpu::GpuMat &dst)\r
 282 {\r
 283     Rect dst_roi = buildMaps(src.size(), K, R, d_xmap_, d_ymap_);\r
 284     dst.create(dst_roi.height + 1, dst_roi.width + 1, src.type());\r
 285     gpu::remap(src, dst, d_xmap_, d_ymap_, interp_mode, border_mode);\r
 286     return dst_roi.tl();\r
 287 }\r
 288 #endif\r
 289 \r
 290 } // namespace detail\r
 291 } // namespace cv\r