helpers.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Georgia Institute of Technology
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Georgia Institute of Technology nor the names of
00018  *     its contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef cpl_visual_features_helpers_h_DEFINED
00036 #define cpl_visual_features_helpers_h_DEFINED
00037 
00038 #include <iostream>
00039 #include <fstream>
00040 
00041 #include <math.h>
00042 
00043 #include <boost/scoped_array.hpp>
00044 #include <boost/shared_array.hpp>
00045 
00046 #include <opencv2/core/core.hpp>
00047 #include <opencv2/imgproc/imgproc.hpp>
00048 #include <opencv2/highgui/highgui.hpp>
00049 
00050 namespace cpl_visual_features
00051 {
00052 
00053 typedef float (*kernel)(const float&, const float&);
00054 
00055 cv::Mat downSample(cv::Mat data_in, int scales)
00056 {
00057   cv::Mat out = data_in.clone();
00058   for (int i = 0; i < scales; ++i)
00059   {
00060     cv::pyrDown(data_in, out);
00061     data_in = out;
00062   }
00063   return out;
00064 }
00065 
00066 cv::Mat upSample(cv::Mat data_in, int scales)
00067 {
00068   cv::Mat out = data_in.clone();
00069   for (int i = 0; i < scales; ++i)
00070   {
00071     // NOTE: Currently assumes even cols, rows for data_in
00072     cv::Size out_size(data_in.cols*2, data_in.rows*2);
00073     cv::pyrUp(data_in, out, out_size);
00074     data_in = out;
00075   }
00076   return out;
00077 }
00078 
00079 double subPIAngle(double theta)
00080 {
00081   while (theta > M_PI)
00082   {
00083     theta -= 2.0*M_PI;
00084   }
00085   while (theta < -M_PI)
00086   {
00087     theta += 2.0*M_PI;
00088   }
00089   return theta;
00090 }
00091 
00092 float Cubic(const float& x, const float& scale)
00093 {
00094   /*
00095   % See Keys, "Cubic Convolution Interpolation for Digital Image
00096   % Processing," IEEE Transactions on Acoustics, Speech, and Signal
00097   % Processing, Vol. ASSP-29, No. 6, December 1981, p. 1155.
00098   */
00099 
00100   float absx = fabs(x*scale);
00101   float absx2 = pow(absx, 2);
00102   float absx3 = pow(absx, 3);
00103 
00104   float f = (1.5*absx3 - 2.5*absx2 + 1) * (absx <= 1) +
00105             (-0.5*absx3 + 2.5*absx2 - 4*absx + 2) *
00106             ((1 < absx) & (absx <= 2));
00107 
00108   return f*scale;
00109 }
00110 
00111 
00112 void Contributions(const unsigned int& in_length, const unsigned int& out_length, const float& scale,
00113       /*std::function<float (const float&)>*/kernel KernelFoo, const float& kernel_scale, float kernel_width,
00114       const bool antialiasing, boost::shared_array<float>& weights, boost::shared_array<int>& indices,
00115       unsigned int& P)
00116 {
00117   float sum;
00118   float* weights_ptr, *weights_temp_ptr;
00119   int* indices_ptr, *indices_temp_ptr;
00120 
00121   P = ceil(kernel_width) + 2;
00122 
00123   boost::scoped_array<bool> kill_col(new bool[P]);
00124   for (unsigned int y = 0; y < P; y++) {
00125     kill_col[y] = true;
00126   }
00127 
00128   weights.reset(new float[out_length*P]);
00129   indices.reset(new int[out_length*P]);
00130 
00131   for (unsigned int x = 1; x <= out_length; x++) {
00132     float u = x/scale + (0.5 * (1 - 1/scale));
00133     float left = floor(u - kernel_width/2);
00134 
00135     sum = 0.0;
00136 
00137     weights_ptr = weights.get() + ((x-1)*P);
00138     indices_ptr = indices.get() + ((x-1)*P);
00139 
00140     for (unsigned int y = 0; y < P; y++) {
00141       indices_ptr[y] = left + y - 1;
00142 
00143       weights_ptr[y] = KernelFoo(u - (indices_ptr[y] + 1), kernel_scale);
00144       sum += weights_ptr[y];
00145 
00146       if (indices_ptr[y] < 0)
00147         indices_ptr[y] = 0;
00148       else if(indices_ptr[y] >= in_length)
00149         indices_ptr[y] = in_length - 1;
00150     }
00151 
00152     for (unsigned int y = 0; y < P; y++) {
00153       weights_ptr[y] /= sum;
00154 
00155       kill_col[y] = (kill_col[y] && (weights_ptr[y] == 0));
00156     }
00157   }
00158 
00159 
00160   // Remove the kill columns from both weights and indices
00161   boost::scoped_array<unsigned int> relocate_idx(new unsigned int[P]);
00162   unsigned int max_idx = 0;
00163   for (unsigned int y = 0; y < P; y++) {
00164     if (!kill_col[y])
00165       relocate_idx[max_idx++] = y;
00166   }
00167 
00168   if (max_idx < P) {
00169     boost::shared_array<float> weights_temp(new float[out_length*max_idx]);
00170     boost::shared_array<int> indices_temp(new int[out_length*max_idx]);
00171 
00172     for (unsigned int x = 0; x < out_length; x++) {
00173       weights_ptr = weights.get() + (x*P);
00174       indices_ptr = indices.get() + (x*P);
00175 
00176       weights_temp_ptr = weights_temp.get() + (x*max_idx);
00177       indices_temp_ptr = indices_temp.get() + (x*max_idx);
00178 
00179       for (unsigned int y = 0; y < max_idx; y++) {
00180         weights_temp_ptr[y] = weights_ptr[relocate_idx[y]];
00181         indices_temp_ptr[y] = indices_ptr[relocate_idx[y]];
00182       }
00183     }
00184 
00185     weights = weights_temp;
00186     indices = indices_temp;
00187   }
00188 
00189   P = max_idx;
00190 }
00191 
00192 
00193 template <class T>
00194 void ResizeAlongDim(const cv::Mat& in, const unsigned int& dim, const boost::shared_array<float>& weights, const boost::shared_array<int>& indices, const unsigned int& out_length, const unsigned int& P, cv::Mat& out)
00195 {
00196   float sum;
00197   int index;
00198   unsigned int in_gap_next_pxl, out_gap_next_pxl;
00199   unsigned int limit;
00200 
00201   if (dim == 0) {
00202     // for height rescaling
00203 
00204     out = cv::Mat::zeros(out_length, in.cols, in.type());
00205     limit = in.cols;
00206 
00207     in_gap_next_pxl = in.cols*in.channels();
00208     out_gap_next_pxl = out.cols*in.channels();
00209   } else {
00210     // for width rescaling
00211 
00212     out = cv::Mat::zeros(in.rows, out_length, in.type());
00213     limit = in.rows;
00214 
00215     in_gap_next_pxl = in.channels();
00216     out_gap_next_pxl = in.channels();
00217   }
00218 
00219   T* in_ptr = (T*)in.data;
00220   T* out_ptr = (T*)out.data;
00221   unsigned int ch = in.channels();
00222 
00223   for (unsigned int k = 0; k < limit; k++) {
00224     for (unsigned int m = 0; m < out_length; m++) {
00225       for (unsigned int c = 0; c < ch; c++) {
00226         sum = 0.0;
00227 
00228         for (unsigned int p = 0; p < P; p++) {
00229           index = indices[(m*P) + p];
00230           sum += weights[(m*P) + p] * in_ptr[(index * in_gap_next_pxl) + c];
00231         }
00232 
00233         out_ptr[(m * out_gap_next_pxl) + c] = sum; //(sum < 0 ? 0 : sum);
00234       }
00235     }
00236 
00237     if (dim == 0) {
00238       in_ptr += ch;
00239       out_ptr += ch;
00240     } else {
00241       in_ptr += in.cols*ch;
00242       out_ptr += out.cols*ch;
00243     }
00244   }
00245 }
00246 
00247 void imResize(const cv::Mat& in_im, const float& scale, cv::Mat& out_im)
00248 {
00249   cv::Mat src_im, intermediate_out;
00250   float kernel_scale;
00251   float kernel_width = 4;
00252   bool antialiasing = true;
00253   unsigned int rscl_h = ceil(in_im.rows*scale);
00254   unsigned int rscl_w = ceil(in_im.cols*scale);
00255   unsigned int P;
00256 
00257   // convert input matrix to float
00258   int src_type = in_im.type();
00259   in_im.convertTo(src_im, CV_32FC(in_im.channels()));
00260 
00261   boost::shared_array<float> weights;
00262   boost::shared_array<int> indices;
00263 
00264   if ((scale < 1) && antialiasing) {
00265     // Use a modified kernel to simultaneously interpolate and
00266     // antialias.
00267     //auto fooH = [&](const float& x) -> float { return (scale * Cubic(scale*x)); };
00268     kernel_width = kernel_width / scale;
00269     kernel_scale = scale;
00270   } else {
00271     // No antialiasing; use unmodified kernel.
00272     kernel_scale = 1.0;
00273   }
00274 
00275   Contributions(src_im.rows, rscl_h, scale, &Cubic, kernel_scale, kernel_width, antialiasing, weights, indices, P);
00276 
00277   if (src_im.type() == CV_8UC1 || src_im.type() == CV_8UC3)
00278     ResizeAlongDim<unsigned char>(src_im, 0, weights, indices, rscl_h, P, intermediate_out);
00279   else
00280     ResizeAlongDim<float>(src_im, 0, weights, indices, rscl_h, P, intermediate_out);
00281 
00282   Contributions(src_im.cols, rscl_w, scale, &Cubic, kernel_scale, kernel_width, antialiasing, weights, indices, P);
00283 
00284   if (src_im.type() == CV_8UC1 || src_im.type() == CV_8UC3)
00285     ResizeAlongDim<unsigned char>(intermediate_out, 1, weights, indices, rscl_w, P, out_im);
00286   else
00287     ResizeAlongDim<float>(intermediate_out, 1, weights, indices, rscl_w, P, out_im);
00288 
00289   /*
00290   int x = 10, y = 1;
00291   std::cout << out_im.at<cv::Vec3f>(x,y)[0] << "," << out_im.at<cv::Vec3f>(x,y)[1] << "," << out_im.at<cv::Vec3f>(x,y)[2] << std::endl;
00292 
00293   // if the mat is going to be converted to unsigned char - prepare for rounding
00294   if (src_type == CV_8UC1) {
00295     out_im += cv::Scalar(1e-5);
00296   } else if (src_type == CV_8UC3) {
00297     out_im += cv::Scalar(1e-5, 1e-5, 1e-5);
00298   }
00299 
00300   std::cout << out_im.at<cv::Vec3f>(x,y)[0] << "," << out_im.at<cv::Vec3f>(x,y)[1] << "," << out_im.at<cv::Vec3f>(x,y)[2] << std::endl;
00301   */
00302 
00303   // convert output to original src type
00304   out_im.convertTo(out_im, src_type);
00305 
00306   //std::cout << (int)out_im.at<cv::Vec3b>(x,y)[0] << "," << (int)out_im.at<cv::Vec3b>(x,y)[1] << "," << (int)out_im.at<cv::Vec3b>(x,y)[2] << std::endl;
00307 }
00308 
00309 };
00310 #endif // cpl_visual_features_helpers_h_DEFINED


cpl_visual_features
Author(s): Tucker Hermans
autogenerated on Wed Nov 27 2013 11:52:36