/*************************************************************************
 *  
 *  
 *  Copyright 2014  Adam Harmat (McGill University) 
 *                      [adam.harmat@mail.mcgill.ca]
 *                  Michael Tribou (University of Waterloo)
 *                      [mjtribou@uwaterloo.ca]
 *
 *  Multi-Camera Parallel Tracking and Mapping (MCPTAM) is free software:
 *  you can redistribute it and/or modify it under the terms of the GNU 
 *  General Public License as published by the Free Software Foundation,
 *  either version 3 of the License, or (at your option) any later
 *  version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *  
 *  MCPTAM is based on the Parallel Tracking and Mapping (PTAM) software.
 *  Copyright 2008 Isis Innovation Limited
 *  
 *  
 ************************************************************************/


/****************************************************************************************
 *
 * \file PatchFinder.h
 * \brief Declaration of the PatchFinder class
 *
 * This code is from the original PTAM, which is
 * Copyright 2008 Isis Innovation Limited
 *
 * This header declares the PatchFinder class.
 * This is quite a low-level class.
 * 
 * The purpose of the PatchFinder is to find a map point in a new view. 
 * This proceeds in multiple stages:
 * 1. A warping matrix appropriate for the current view is calculated,
 * 2. A `coarse' matching template of the map point is generated by warping;
 * 3. The new view is searched for this template at corner locations;
 * 4. An inverse-composition (`fine') matching template is generated;
 * 5. A sub-pixel accurate position is calculated using inverse composition.
 *
 * To clarify points 1 and 2 above: Each map point does _not_ store a `patch' 
 * in the style of many other SLAM implementations! Each map point came from 
 * a key-frame, and that key-frame is stored in the map. Each map point only
 * stores the information on where it came from in what key-frame. Patches 
 * are then generated from the pixels of the source key-frame. According to the
 * relative camera poses now and of the source key-frame, a warp has to be 
 * applied to generate an NxN pixel square template appropriate for the current 
 * view. Thus, the matching template is always NxN, but the region of source 
 * pixels used can change size and shape! This is all described in the 
 * Klein & Murray ISMAR 2007.
 *
 * Most of the above stages are optional, and there are different versions
 * of stages 1/2, including some for operating when there is not yet a 3D map 
 * point. The class provides no safety checks to ensure that the operations
 * are carried out in the right order - it's the caller's responsibility.
 *
 * The patch finder uses zero-mean SSD as its difference metric.
 *
 * Although PatchFinder can use arbitrary-sized search templates (it's determined
 * at construction), the use of 8x8 pixel templates (the default) is highly 
 * recommended, as the coarse search for this size is SSE-optimised.
 *
 ****************************************************************************************/

#ifndef __PATCHFINDER_H
#define __PATCHFINDER_H

#include <mcptam/LevelHelpers.h>
#include <TooN/TooN.h>
#include <TooN/se3.h>
#include <cvd/image.h>
#include <cvd/byte.h>

class MapPoint;
class KeyFrame;

/** @brief Helps in finding a MapPoint in a new view.
 * 
 * This class is pretty complex and low-level. As I didn't have to make any modifications to it, I have
 * not read or understood most of the code, and thus can't generate good documentation for it. If you
 * need to tinker with this class, you'll have to dive into the code on your own. */
class PatchFinder
{
public:
  /// Constructor defines size of search patch.
  PatchFinder(int nPatchSize = 8);
  
  // Step 1 Function.
  /** @brief This calculates the warping matrix appropriate for observing point p from the current view 
   * 
   * This also needs the camera projection derivates at level zero for the point's projection in the new view.
   * It also calculates which pyramid level we should search in, and this is returned as an int. Negative level returned denotes an inappropriate 
   * transformation. */
  int CalcSearchLevelAndWarpMatrix(MapPoint &point, TooN::SE3<> se3CFromW, TooN::Matrix<2> &m2CamDerivs);
  
  /** @brief Get the search level */
  inline int GetLevel() { return mnSearchLevel; }
  
  /** @brief Get the scale of the search level */
  inline int GetLevelScale() { return LevelScale(mnSearchLevel); }
  
  
  // Step 2 Functions
  // Generates the NxN search template either from the pre-calculated warping matrix, or an identity transformation.
  
  /// If the warping matrix has already been pre-calced, use this.
  void MakeTemplateCoarseCont(MapPoint &point); 
  
  /// This also calculates the warp.
  void MakeTemplateCoarse(MapPoint &point, TooN::SE3<> se3CFromW, TooN::Matrix<2> &m2CamDerivs); 

  /// Identity warp: just copies pixels from the source KF.
  void MakeTemplateCoarseNoWarp(MapPoint &point);  
  
  /// Identity warp if no MapPoint struct exists yet.
  void MakeTemplateCoarseNoWarp(KeyFrame &kf, int nLevel, CVD::ImageRef irLevelPos); 

  /// If the template making failed (i.e. it needed pixels outside the source image), this bool will return false.
  inline bool TemplateBad()      { return mbTemplateBad;} 
  
  // Step 3 Functions
  /** @brief This is the raison d'etre of the class: finds the patch in the current input view,
   * centered around ir, Searching around FAST corner locations only within a radius nRange only.
   *
   * Inputs are given in level-zero coordinates! Returns true if the patch was found. */
  bool FindPatchCoarse(CVD::ImageRef ir, KeyFrame &kf, unsigned int nRange, int& nScore, bool bExhaustive = false);  
  
  /// This evaluates the score at one location
  int ZMSSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir); 

  // Results from step 3:
  // All positions are in the scale of level 0.

  /// Get the coarse position of the patch
  inline CVD::ImageRef GetCoarsePos() { return CVD::ImageRef((int) mv2CoarsePos[0], (int) mv2CoarsePos[1]);} 

  /// Get the coarse position of the patch as a vector
  inline TooN::Vector<2> GetCoarsePosAsVector() { return mv2CoarsePos; }
  
  // Step 4
  /// Generate the inverse composition template and jacobians
  void MakeSubPixTemplate();  
  
  // Step 5 Functions
  /// Run inverse composition till convergence
  bool IterateSubPixToConvergence(KeyFrame &kf, int nMaxIts, double dConvLimit=0.03);  
  
  /// Single iteration of IC. Returns sum-squared pixel update dist, or negative if out of image
  double IterateSubPix(KeyFrame &kf);     

  /// Get subpixel result
  inline TooN::Vector<2> GetSubPixPos()  { return mv2SubPixPos;   }  
  
  /// Set starting point for subpixel refinement
  void SetSubPixPos(TooN::Vector<2> v2)  { mv2SubPixPos = v2; mdMeanDiff = 0;    }  
  
  /** @brief Get the uncertainty estimate of a found patch
   *
   * Return the uncertainty for found patch position. This just returns an appropriately-scaled identity! */
  inline TooN::Matrix<2> GetCov()
  {
    return LevelScale(mnSearchLevel) * TooN::Identity;
  };
  
  /// Get the template for this patch
  inline CVD::Image<CVD::byte> GetTemplate()
  {
    return mimTemplate;
  };
  
  int mnMaxSSD; ///< This is the max ZMSSD for a valid match. It's set in the constructor.

  static int snMaxSSDPerPixel; ///< Max ZMSSD for a valid match normalized per pixel

protected:
 
  int mnPatchSize; ///< Size of one side of the matching template.
  
  // Some values stored for the coarse template:
  int mnTemplateSum;    ///< Cached pixel-sum of the coarse template
  int mnTemplateSumSq;  ///< Cached pixel-squared sum of the coarse template
  inline void MakeTemplateSums(); ///< Calculate template sums
  
  CVD::Image<CVD::byte> mimTemplate;   ///< The matching template
  CVD::Image<std::pair<float,float> > mimJacs;  ///< Inverse composition jacobians; stored as floats to save a bit of space.
  
  TooN::Matrix<2> mm2WarpInverse;   ///< Warping matrix
  int mnSearchLevel;          ///< Search level in input pyramid
  TooN::Matrix<3> mm3HInv;          ///< Inverse composition JtJ^-1
  TooN::Vector<2> mv2SubPixPos;     ///< In the scale of level 0
  double mdMeanDiff;          ///< Updated during inverse composition
  
  CVD::ImageRef mirPredictedPos;  ///< Search center location of FindPatchCoarse in L0
  TooN::Vector<2> mv2CoarsePos;     ///< In the scale of level 0; hence the use of vector rather than ImageRef
  CVD::ImageRef mirCenter;    ///< Quantized location of the center pixel of the NxN pixel template
  bool mbFound;               ///< Was the patch found?
  bool mbTemplateBad;         ///< Error during template generation?

  // Some cached values to avoid duplicating work if the camera is stopped:
  MapPoint *mpLastTemplateMapPoint;  ///< Which was the last map point this PatchFinder used?
  TooN::Matrix<2> mm2LastWarpMatrix;       ///< What was the last warp matrix this PatchFinder used?
  
};

#endif

