mv_image/image/src/Tracking/MFTracker.cpp

   1 /**
   2  * Copyright (c) 2015 Samsung Electronics Co., Ltd All Rights Reserved
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 #include "Tracking/MFTracker.h"
  18
  19 #include <opencv/cv.h>
  20
  21 namespace MediaVision {
  22 namespace Image {
  23 namespace {
  24         const float FloatEps = 10e-6f;
  25
  26         template<typename T>
  27         T getMedian(std::vector<T>& values, int size = -1) {
  28                 if (size == -1)
  29                         size = (int)values.size();
  30
  31                 std::vector<T> copy(values.begin(), values.begin() + size);
  32                 std::sort(copy.begin(), copy.end());
  33                 if (size%2 == 0)
  34                         return (copy[size/2-1]+copy[size/2])/((T)2.0);
  35                 else
  36                         return copy[(size - 1) / 2];
  37         }
  38
  39         inline float l2distance(cv::Point2f p1, cv::Point2f p2) {
  40                 const float dx = p1.x - p2.x;
  41                 const float dy = p1.y - p2.y;
  42                 return sqrtf(dx * dx + dy * dy);
  43         }
  44 } /* anonymous namespace */
  45
  46 MFTracker::Params::Params()
  47 {
  48         mPointsInGrid = 10;
  49         mWindowSize = cv::Size(3, 3);
  50         mPyrMaxLevel = 5;
  51 }
  52
  53 MFTracker::MFTracker(Params params) :
  54                 m_isInit(false),
  55                 m_params(params),
  56                 m_termcrit(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.3),
  57                 m_confidence(0.0f)
  58 {
  59 }
  60
  61 bool MFTracker::track(const cv::Mat& frame, std::vector<cv::Point>& result)
  62 {
  63         result.clear();
  64
  65         if (!m_isInit) {
  66                 if (m_startLocation.empty())
  67                         return false;
  68
  69                 if (!init(frame))
  70                         return false;
  71         } else {
  72                 if (!update(frame)) {
  73                         m_isInit = false;
  74                         m_startLocation.clear();
  75                         return false;
  76                 }
  77         }
  78
  79         const size_t numberOfContourPoints = m_startLocation.size();
  80         result.resize(numberOfContourPoints);
  81
  82         for (size_t i = 0; i < numberOfContourPoints; ++i) {
  83                 result[i].x = (int)(m_boundingBox.x +
  84                                         m_startLocation[i].x * m_boundingBox.width);
  85                 result[i].y = (int)(m_boundingBox.y +
  86                                         m_startLocation[i].y * m_boundingBox.height);
  87         }
  88
  89         return true;
  90 }
  91
  92 void MFTracker::reinforcement(const std::vector<cv::Point>& location)
  93 {
  94         m_isInit = false;
  95
  96         if (location.size() < 3) {
  97                 m_startLocation.clear();
  98                 m_boundingBox.x = 0;
  99                 m_boundingBox.y = 0;
 100                 m_boundingBox.width = 0;
 101                 m_boundingBox.height = 0;
 102
 103                 return;
 104         }
 105
 106         const cv::Rect_<float>& boundingBox = cv::boundingRect(location);
 107         m_boundingBox = boundingBox;
 108
 109         const size_t numberOfContourPoints = location.size();
 110         m_startLocation.resize(numberOfContourPoints);
 111         for (size_t i = 0; i < numberOfContourPoints; ++i) {
 112                 m_startLocation[i].x = (location[i].x - boundingBox.x) / boundingBox.width;
 113                 m_startLocation[i].y = (location[i].y - boundingBox.y) / boundingBox.height;
 114         }
 115 }
 116
 117 cv::Ptr<ObjectTracker> MFTracker::clone() const
 118 {
 119         return cv::Ptr<ObjectTracker>(new MFTracker(*this));
 120 }
 121
 122 bool MFTracker::init(const cv::Mat& image)
 123 {
 124         if (image.empty())
 125                 return false;
 126
 127         image.copyTo(m_image);
 128         buildOpticalFlowPyramid(
 129                         m_image,
 130                         m_pyramid,
 131                         m_params.mWindowSize,
 132                         m_params.mPyrMaxLevel);
 133
 134         m_isInit = true;
 135         return m_isInit;
 136 }
 137
 138 bool MFTracker::update(const cv::Mat& image)
 139 {
 140         if (!m_isInit || image.empty())
 141                 return false;
 142
 143         /* Handles such behaviour when preparation frame has the size
 144          * different to the tracking frame size. In such case, we resize preparation
 145          *frame and bounding box. Then, track as usually:
 146          */
 147         if (m_image.rows != image.rows || m_image.cols != image.cols) {
 148                 const float xFactor = (float) image.cols / m_image.cols;
 149                 const float yFactor = (float) image.rows / m_image.rows;
 150
 151                 resize(m_image, m_image, cv::Size(), xFactor, yFactor);
 152
 153                 m_boundingBox.x *= xFactor;
 154                 m_boundingBox.y *= yFactor;
 155                 m_boundingBox.width *= xFactor;
 156                 m_boundingBox.height *= yFactor;
 157         }
 158
 159         cv::Mat oldImage = m_image;
 160
 161         cv::Rect_<float> oldBox = m_boundingBox;
 162         if (!medianFlowImpl(oldImage, image, oldBox))
 163                 return false;
 164
 165         image.copyTo(m_image);
 166         m_boundingBox = oldBox;
 167
 168         return true;
 169 }
 170
 171 bool MFTracker::isInited() const
 172 {
 173         return m_isInit;
 174 }
 175
 176 float MFTracker::getLastConfidence() const
 177 {
 178         return m_confidence;
 179 }
 180
 181 cv::Rect_<float> MFTracker::getLastBoundingBox() const
 182 {
 183         return m_boundingBox;
 184 }
 185
 186 bool MFTracker::medianFlowImpl(
 187                 cv::Mat oldImage_gray, cv::Mat newImage_gray, cv::Rect_<float>& oldBox)
 188 {
 189         std::vector<cv::Point2f> pointsToTrackOld, pointsToTrackNew;
 190
 191         const float gridXStep = oldBox.width / m_params.mPointsInGrid;
 192         const float gridYStep = oldBox.height / m_params.mPointsInGrid;
 193         for (int i = 0; i < m_params.mPointsInGrid; i++) {
 194                 for (int j = 0; j < m_params.mPointsInGrid; j++) {
 195                         pointsToTrackOld.push_back(
 196                                         cv::Point2f(oldBox.x + .5f*gridXStep + 1.f*gridXStep*j,
 197                                                         oldBox.y + .5f*gridYStep + 1.f*gridYStep*i));
 198                 }
 199         }
 200
 201         const size_t numberOfPointsToTrackOld = pointsToTrackOld.size();
 202         std::vector<uchar> status(numberOfPointsToTrackOld);
 203         std::vector<float> errors(numberOfPointsToTrackOld);
 204
 205         std::vector<cv::Mat> tempPyramid;
 206         cv::buildOpticalFlowPyramid(
 207                                                         newImage_gray,
 208                                                         tempPyramid,
 209                                                         m_params.mWindowSize,
 210                                                         m_params.mPyrMaxLevel);
 211
 212         cv::calcOpticalFlowPyrLK(m_pyramid,
 213                                                         tempPyramid,
 214                                                         pointsToTrackOld,
 215                                                         pointsToTrackNew,
 216                                                         status,
 217                                                         errors,
 218                                                         m_params.mWindowSize,
 219                                                         m_params.mPyrMaxLevel,
 220                                                         m_termcrit);
 221
 222         std::vector<cv::Point2f> di;
 223         for (size_t idx = 0u; idx < numberOfPointsToTrackOld; idx++) {
 224                 if (status[idx] == 1)
 225                         di.push_back(pointsToTrackNew[idx] - pointsToTrackOld[idx]);
 226         }
 227
 228         std::vector<bool> filter_status;
 229         check_FB(tempPyramid,
 230                         pointsToTrackOld,
 231                         pointsToTrackNew,
 232                         filter_status);
 233
 234         check_NCC(oldImage_gray,
 235                         newImage_gray,
 236                         pointsToTrackOld,
 237                         pointsToTrackNew,
 238                         filter_status);
 239
 240         for (size_t idx = 0u; idx < pointsToTrackOld.size(); idx++) {
 241                 if (!filter_status[idx]) {
 242                         pointsToTrackOld.erase(pointsToTrackOld.begin() + idx);
 243                         pointsToTrackNew.erase(pointsToTrackNew.begin() + idx);
 244                         filter_status.erase(filter_status.begin() + idx);
 245                         idx--;
 246                 }
 247         }
 248
 249         if (pointsToTrackOld.empty() || di.empty())
 250                 return false;
 251
 252         cv::Point2f mDisplacement;
 253         cv::Rect_<float> boxCandidate =
 254                                 vote(pointsToTrackOld, pointsToTrackNew, oldBox, mDisplacement);
 255
 256         std::vector<float> displacements;
 257         for (size_t idx = 0u; idx < di.size(); idx++) {
 258                 di[idx] -= mDisplacement;
 259                 displacements.push_back(sqrt(di[idx].ddot(di[idx])));
 260         }
 261
 262         m_confidence = (10.f - getMedian(displacements, (int)displacements.size())) / 10.f;
 263
 264         if (m_confidence < 0.f) {
 265                 m_confidence = 0.f;
 266                 return false;
 267         }
 268
 269         m_pyramid.swap(tempPyramid);
 270         oldBox = boxCandidate;
 271         return true;
 272 }
 273
 274 cv::Rect_<float> MFTracker::vote(
 275                 const std::vector<cv::Point2f>& oldPoints,
 276                 const std::vector<cv::Point2f>& newPoints,
 277                 const cv::Rect_<float>& oldRect,
 278                 cv::Point2f& mD)
 279 {
 280         cv::Rect_<float> newRect;
 281         cv::Point2f newCenter(
 282                         oldRect.x + oldRect.width / 2.f,
 283                         oldRect.y + oldRect.height / 2.f);
 284
 285         const int n = (int)oldPoints.size();
 286         std::vector<float> buf(std::max(n*(n-1) / 2, 3), 0.f);
 287
 288         if (oldPoints.size() == 1) {
 289                 newRect.x = oldRect.x+newPoints[0].x-oldPoints[0].x;
 290                 newRect.y = oldRect.y+newPoints[0].y-oldPoints[0].y;
 291                 newRect.width = oldRect.width;
 292                 newRect.height = oldRect.height;
 293
 294                 return newRect;
 295         }
 296
 297         float xshift = 0.f;
 298         float yshift = 0.f;
 299         for (int i = 0; i < n; i++)
 300                 buf[i] = newPoints[i].x - oldPoints[i].x;
 301
 302         xshift = getMedian(buf, n);
 303         newCenter.x += xshift;
 304         for (int idx = 0; idx < n; idx++)
 305                 buf[idx] = newPoints[idx].y - oldPoints[idx].y;
 306
 307         yshift = getMedian(buf, n);
 308         newCenter.y += yshift;
 309         mD = cv::Point2f(xshift, yshift);
 310
 311         if (oldPoints.size() == 1) {
 312                 newRect.x = newCenter.x - oldRect.width / 2.f;
 313                 newRect.y = newCenter.y - oldRect.height / 2.f;
 314                 newRect.width = oldRect.width;
 315                 newRect.height = oldRect.height;
 316
 317                 return newRect;
 318         }
 319
 320         float nd = 0.f;
 321         float od = 0.f;
 322         for (int i = 0, ctr = 0; i < n; i++) {
 323                 for (int j = 0; j < i; j++) {
 324                         nd = l2distance(newPoints[i], newPoints[j]);
 325                         od = l2distance(oldPoints[i], oldPoints[j]);
 326                         buf[ctr] = (od == 0.f ? 0.f : nd / od);
 327                         ctr++;
 328                 }
 329         }
 330
 331         float scale = getMedian(buf, n*(n-1) / 2);
 332         newRect.x = newCenter.x - scale * oldRect.width / 2.f;
 333         newRect.y = newCenter.y-scale * oldRect.height / 2.f;
 334         newRect.width = scale * oldRect.width;
 335         newRect.height = scale * oldRect.height;
 336
 337         return newRect;
 338 }
 339
 340 void MFTracker::check_FB(
 341                 std::vector<cv::Mat> newPyramid,
 342                 const std::vector<cv::Point2f>& oldPoints,
 343                 const std::vector<cv::Point2f>& newPoints,
 344                 std::vector<bool>& status)
 345 {
 346         const size_t numberOfOldPoints = oldPoints.size();
 347
 348         if (status.empty())
 349                 status = std::vector<bool>(numberOfOldPoints, true);
 350
 351         std::vector<uchar> LKstatus(numberOfOldPoints);
 352         std::vector<float> errors(numberOfOldPoints);
 353         std::vector<float> FBerror(numberOfOldPoints);
 354         std::vector<cv::Point2f> pointsToTrackReprojection;
 355
 356         calcOpticalFlowPyrLK(newPyramid,
 357                                                 m_pyramid,
 358                                                 newPoints,
 359                                                 pointsToTrackReprojection,
 360                                                 LKstatus,
 361                                                 errors,
 362                                                 m_params.mWindowSize,
 363                                                 m_params.mPyrMaxLevel,
 364                                                 m_termcrit);
 365
 366         for (size_t idx = 0u; idx < numberOfOldPoints; idx++)
 367                 FBerror[idx] = l2distance(oldPoints[idx], pointsToTrackReprojection[idx]);
 368
 369         float FBerrorMedian = getMedian(FBerror) + FloatEps;
 370         for (size_t idx = 0u; idx < numberOfOldPoints; idx++)
 371                 status[idx] = (FBerror[idx] < FBerrorMedian);
 372 }
 373
 374 void MFTracker::check_NCC(
 375                 const cv::Mat& oldImage,
 376                 const cv::Mat& newImage,
 377                 const std::vector<cv::Point2f>& oldPoints,
 378                 const std::vector<cv::Point2f>& newPoints,
 379                 std::vector<bool>& status)
 380 {
 381         std::vector<float> NCC(oldPoints.size(), 0.f);
 382         cv::Size patch(30, 30);
 383         cv::Mat p1;
 384         cv::Mat p2;
 385
 386         for (size_t idx = 0u; idx < oldPoints.size(); idx++) {
 387                 getRectSubPix(oldImage, patch, oldPoints[idx], p1);
 388                 getRectSubPix(newImage, patch, newPoints[idx], p2);
 389
 390                 const int N = 900;
 391                 const float s1 = sum(p1)(0);
 392                 const float s2 = sum(p2)(0);
 393                 const float n1 = norm(p1);
 394                 const float n2 = norm(p2);
 395                 const float prod = p1.dot(p2);
 396                 const float sq1 = sqrt(n1 * n1 - s1 * s1 / N);
 397                 const float sq2 = sqrt(n2 * n2 - s2 * s2 / N);
 398                 NCC[idx] = (sq2 == 0 ? sq1 / std::abs(sq1) : (prod - s1 * s2 / N) / sq1 / sq2);
 399         }
 400
 401         float median = getMedian(NCC) - FloatEps;
 402         for (size_t idx = 0u; idx < oldPoints.size(); idx++)
 403                 status[idx] = status[idx] && (NCC[idx] > median);
 404 }
 405
 406 } /* Image */
 407 } /* MediaVision */