dense_lk.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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 #include <cpl_visual_features/motion/dense_lk.h>
00036 #include <opencv2/imgproc/imgproc.hpp>
00037 
00038 namespace cpl_visual_features
00039 {
00040 
00041 DenseLKFlow::DenseLKFlow(int win_size, int num_levels) :
00042     win_size_(win_size), max_level_(num_levels-1)
00043 {
00044   // Create derivative kernels for flow calculation
00045   cv::getDerivKernels(dy_kernel_, dx_kernel_, 1, 0, CV_SCHARR, true, CV_32F);
00046   cv::flip(dy_kernel_, dy_kernel_, -1);
00047   g_kernel_ = cv::getGaussianKernel(3, 2.0, CV_32F);
00048   optic_g_kernel_ = cv::getGaussianKernel(5, 10.0, CV_32F);
00049   cv::transpose(dy_kernel_, dx_kernel_);
00050 }
00051 
00052 DenseLKFlow::~DenseLKFlow()
00053 {
00054 }
00055 
00056 std::vector<cv::Mat> DenseLKFlow::operator()(cv::Mat& cur_frame,
00057                                              cv::Mat& prev_frame,
00058                                              bool color_in)
00059 {
00060   // Convert to grayscale
00061   if (color_in)
00062   {
00063     cv::Mat tmp_bw(cur_frame.size(), CV_8UC1);
00064     cv::Mat cur_bw(cur_frame.size(), CV_32FC1);
00065     cv::Mat prev_bw(prev_frame.size(), CV_32FC1);
00066     cv::cvtColor(cur_frame, tmp_bw, CV_BGR2GRAY);
00067     tmp_bw.convertTo(cur_bw, CV_32FC1, 1.0/255, 0);
00068     cv::cvtColor(prev_frame, tmp_bw, CV_BGR2GRAY);
00069     tmp_bw.convertTo(prev_bw, CV_32FC1, 1.0/255, 0);
00070     return hierarchy(cur_bw, prev_bw);
00071   }
00072   else
00073   {
00074     return hierarchy(cur_frame, prev_frame);
00075   }
00076 }
00077 
00078 std::vector<cv::Mat> DenseLKFlow::hierarchy(cv::Mat& f2, cv::Mat& f1)
00079 {
00080   const int divisor = std::pow(2,max_level_);
00081   cv::Mat Dx(f2.rows/divisor, f2.cols/divisor, CV_32FC1, cv::Scalar(0.0));
00082   cv::Mat Dy = Dx.clone();
00083   std::vector<cv::Mat> g1s;
00084   std::vector<cv::Mat> g2s;
00085   cv::buildPyramid(f1, g1s, max_level_);
00086   cv::buildPyramid(f2, g2s, max_level_);
00087   std::vector<cv::Mat> flow_outs;
00088 
00089   for (int l = max_level_; l >= 0; --l)
00090   {
00091     if (l != max_level_)
00092     {
00093       Dx = expand(Dx);
00094       Dy = expand(Dy);
00095     }
00096     cv::Mat W = warp(g1s[l], Dx, Dy);
00097     flow_outs = baseLK(g2s[l], W);
00098     Dx = Dx + flow_outs[0];
00099     Dy = Dy + flow_outs[1];
00100     Dx = smooth(Dx);
00101     Dy = smooth(Dy);
00102   }
00103   std::vector<cv::Mat> total_outs;
00104   total_outs.push_back(Dx);
00105   total_outs.push_back(Dy);
00106   return total_outs;
00107 }
00108 
00109 std::vector<cv::Mat> DenseLKFlow::baseLK(cv::Mat& cur_bw, cv::Mat& prev_bw)
00110 {
00111   cv::Mat Ix(cur_bw.size(), CV_32FC1);
00112   cv::Mat Iy(cur_bw.size(), CV_32FC1);
00113   cv::Mat cur_blur(cur_bw.size(), cur_bw.type());
00114   cv::Mat prev_blur(prev_bw.size(), prev_bw.type());
00115   cv::filter2D(cur_bw, cur_blur, CV_32F, g_kernel_);
00116   cv::filter2D(prev_bw, prev_blur, CV_32F, g_kernel_);
00117   cv::Mat It = cur_blur - prev_blur;
00118 
00119   // Get image derivatives
00120   cv::filter2D(cur_bw, Ix, CV_32F, dx_kernel_);
00121   cv::filter2D(cur_bw, Iy, CV_32F, dy_kernel_);
00122 
00123   int win_radius = win_size_/2;
00124   cv::Mat flow_u(cur_bw.size(), CV_32FC1, cv::Scalar(0.0));
00125   cv::Mat flow_v(cur_bw.size(), CV_32FC1, cv::Scalar(0.0));
00126   for (int r = win_radius; r < Ix.rows-win_radius; ++r)
00127   {
00128     for (int c = win_radius; c < Ix.cols-win_radius; ++c)
00129     {
00130       float sIxx = 0.0;
00131       float sIyy = 0.0;
00132       float sIxy = 0.0;
00133       float sIxt = 0.0;
00134       float sIyt = 0.0;
00135       for (int y = r-win_radius; y <= r+win_radius; ++y)
00136       {
00137         for (int x = c-win_radius; x <= c+win_radius; ++x)
00138         {
00139           sIxx += Ix.at<float>(y,x)*Ix.at<float>(y,x);
00140           sIyy += Iy.at<float>(y,x)*Iy.at<float>(y,x);
00141           sIxy += Ix.at<float>(y,x)*Iy.at<float>(y,x);
00142           sIxt += Ix.at<float>(y,x)*It.at<float>(y,x);
00143           sIyt += Iy.at<float>(y,x)*It.at<float>(y,x);
00144         }
00145       }
00146 
00147       const float det = sIxx*sIyy - sIxy*sIxy;
00148       const float trace = sIxx+sIyy;
00149       cv::Vec2f uv;
00150       const double r_score = trace*trace/det;
00151       if (det == 0.0 || r_score > r_thresh_)
00152       {
00153         uv[0] = 0.0;
00154         uv[1] = 0.0;
00155       }
00156       else
00157       {
00158         uv[0] = (-sIyy*sIxt + sIxy*sIyt)/det;
00159         uv[1] = (sIxy*sIxt - sIxx*sIyt)/det;
00160       }
00161       flow_u.at<float>(r,c) = uv[0];
00162       flow_v.at<float>(r,c) = uv[1];
00163     }
00164   }
00165   std::vector<cv::Mat> outs;
00166   outs.push_back(flow_u);
00167   outs.push_back(flow_v);
00168   return outs;
00169 }
00170 
00171 cv::Mat DenseLKFlow::reduce(cv::Mat& input)
00172 {
00173   cv::Mat output;// = input.clone();
00174   cv::pyrDown(input, output);
00175   return output;
00176 }
00177 
00178 cv::Mat DenseLKFlow::expand(cv::Mat& input)
00179 {
00180   cv::Mat output(input.rows*2, input.cols*2, CV_32FC1);
00181   cv::pyrUp(input, output, output.size());
00182   return output;
00183 }
00184 
00185 cv::Mat DenseLKFlow::smooth(cv::Mat& input, int n)
00186 {
00187   cv::Mat sm = input.clone();
00188 #ifdef MEDIAN_FILTER_FLOW
00189   cv::medianBlur(input, sm, 3);
00190 #else // MEDIAN_FILTER_FLOW
00191   for (int l = 0; l < n; ++l)
00192   {
00193     sm = reduce(sm);
00194   }
00195   for (int l = 0; l < n; ++l)
00196   {
00197     sm = expand(sm);
00198   }
00199 #endif // MEDIAN_FILTER_FLOW
00200   return sm;
00201 }
00202 
00203 cv::Mat DenseLKFlow::warp(cv::Mat& i2, cv::Mat& vx, cv::Mat& vy)
00204 {
00205   cv::Mat warpI2(i2.rows, i2.cols, i2.type(), cv::Scalar(0.0));
00206   // cv::Mat warpI3(i2.rows, i2.cols, i2.type(), cv::Scalar(0.0));
00207   cv::Mat map_x(vx.rows, vx.cols, CV_32FC1);
00208   cv::Mat map_y(vy.rows, vy.cols, CV_32FC1);
00209 
00210   for (int y = 0; y < map_x.rows; ++y)
00211   {
00212     for (int x = 0; x < map_x.cols; ++x)
00213     {
00214       map_x.at<float>(y,x) = x + vx.at<float>(y,x);
00215       map_y.at<float>(y,x) = y + vy.at<float>(y,x);
00216     }
00217   }
00218 
00219   // cv::remap(i2, warpI3, map_x, map_y, cv::INTER_NEAREST);
00220   cv::remap(i2, warpI2, map_x, map_y, cv::INTER_LINEAR);
00221 
00222   // for (int y = 0; y < i2.rows; ++y)
00223   // {
00224   //   for (int x = 0; x < i2.cols; ++x)
00225   //   {
00226   //     if (isnan(warpI2.at<float>(y,x)))
00227   //     {
00228   //       warpI2.at<float>(y,x) = warpI3.at<float>(y,x);
00229   //     }
00230   //   }
00231   // }
00232   return warpI2;
00233 }
00234 
00235 //
00236 // Getters and Setters
00237 //
00238 void DenseLKFlow::setWinSize(int win_size)
00239 {
00240   win_size_ = win_size;
00241 }
00242 
00243 void DenseLKFlow::setNumLevels(int num_levels)
00244 {
00245   max_level_ = num_levels-1;
00246 }
00247 }


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