/*
 * Copyright 2017 Jan Dorazil <deu439@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "kptracker.hpp"

KpTracker::KpTracker(InputArray temp, InputArray mask)
{
//   mDetector = ORB::create(
//     ORB_opts.nfeatures,
//     ORB_opts.scale_factor,
//     ORB_opts.nlevels,
//     ORB_opts.edge_thresh,
//     ORB_opts.first_level,
//     ORB_opts.wta_k,
//     ORB_opts.score_type,
//     ORB_opts.patch_size
//   );
  mDetector = AKAZE::create(
    AKAZE_opts.descriptor_type,
    AKAZE_opts.descriptor_size,
    AKAZE_opts.descriptor_channels,
    AKAZE_opts.threshold,
    AKAZE_opts.nOctaves,
    AKAZE_opts.nOctaveLayers,
    AKAZE_opts.diffusivity
  );
  mMatcher = BFMatcher::create(NORM_HAMMING);
  DetectKeypoints(temp, mask, mKp, mDesc);
}

void KpTracker::DetectKeypoints(InputArray img, InputArray mask,
                                std::vector<KeyPoint> &kp, Mat &desc)
{
  mDetector->detectAndCompute(img, mask, kp, desc);
}

void KpTracker::MatchKeypoints(Mat &desc, std::vector<KeyPoint> &kp,
                               std::vector<Point> &pts1,
                               std::vector<Point> &pts2,
                               std::vector<DMatch> &good)
{
  // Clear old data
  pts1.clear();
  pts2.clear();
  good.clear();
  
  // Match keypoints
  MatchType matches;
  mMatcher->knnMatch ( mDesc, desc, matches, 2 );
  
  // Filter matches
  for ( unsigned i = 0; i < matches.size(); i++ )
  {
    if( matches[i].size() < 2)
      break;
    if ( matches[i][0].distance < MATCH_RATIO * matches[i][1].distance )
    {
      pts1.push_back ( mKp[matches[i][0].queryIdx].pt );
      pts2.push_back ( kp[matches[i][0].trainIdx].pt );
      good.push_back ( matches[i][0] );
    }
  }
}


