place_recognition.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
 *  Author: Qinghua Li, Yan Zhuang, Fei Yan
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Intelligent Robotics Lab, DLUT. nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef PLACE_RECOGNITION_H_
#define PLACE_RECOGNITION_H_

#include <QFileDialog>
#include <QMessageBox>
#include <boost/foreach.hpp>
#include "dlut_place_recognition/bearing_angle_image.h"

#define NEIGHBOR 24
#define SCANOCTAVE 3
#define FILTERSCALE 4

typedef struct GlobalFeature
{
  /* Global features */
  double area;         // area of scene
  double duty_cycle;   // duty cycle of scene
} GlobalFeature;

typedef struct SURFDescriptor
{
  int x;
  int y;
  int laplacian;
  double s;
  double dir;
  double mod;
  struct SURFDescriptor* fwd_match;
  void* feature_data;
  double vector[128];
  pcl::PointXYZ cor_point;  // The corresponding position of laser point for this pixel
  std::vector < pcl::PointXYZ > laser_neighbors;  // The neighborhood laser points for this pixel
} SURFDescriptor;

typedef struct SURFPoint
{
  int x;
  int y;
  int laplacian;
  int size;
  int octave;
  int scale;
} SURFPoint;

/* A node in a KD-Tree */
typedef struct SURFKDNode
{
  int ki;                     /* interval index */
  double kv;                  /* interval key value */
  int leaf;                   /* 1 or 0 */
  SURFDescriptor* features;   /* the feature of node */
  int n;                      /* the number of features */
  struct SURFKDNode* kd_left;   /* left child */
  struct SURFKDNode* kd_right;  /* right child */
} SURFKDNode;

typedef struct BbfData
{
  double d;
  void* old_data;
} BbfData;

/* One element in the minimum priority queue */
typedef struct PqNode
{
  void* data;
  int key;
} PqNode;

/* A minimum priority queue */
typedef struct MinPq
{
  PqNode* pq_array;   /* the array of containing priority queue */
  int nallocd;         /* the number of allocating elements */
  int n;               /* the number of elements in queue */
} MinPq;


class PlaceRecognition
{
  public:
    PlaceRecognition ();
    ~PlaceRecognition ();

  public:
    bool extractGlobalFeature (std::vector< std::vector<pcl::PointXYZ> > &points, GlobalFeature* global_feature);

    void cvSURFInitialize ();

    CvSeq* cvSURFDescriptor (const CvArr* _img, CvMemStorage* storage, double quality, int flags = 0);

    bool drawSURFFeatures (IplImage* img, CvSeq* feat, int n);

    void readFeaturesFromFile ();

    CvSeq* findSURFMatchPairs (CvSeq* sep, int n, std::vector<SURFDescriptor> sfeat, CvMemStorage* storage);

    double getMatchDegree (CvSeq* seq);

  private:
    void cvSURFGaussian (CvMat* mat, double s);

    CvSeq* cvFastHessianDetector (const CvMat* sum, CvMemStorage* storage, double quality);

    void cvResizeHaarPattern (int* t_s, int* t_d, int OldSize, int NewSize);

    double cvCalHaarPattern (int* origin, int* t, int widthStep);

    SURFPoint cvSURFPoint (int x, int y, int laplacian, int size, int octave, int scale);

    void drawSURFLoweFeatures (IplImage*, CvSeq*, int);

    void drawSURFLoweFeature (IplImage*, SURFDescriptor*, CvScalar);

    SURFKDNode* buildKDTree (SURFDescriptor* features, int n, int flags);

    SURFKDNode* initKDNode (SURFDescriptor* features, int n);

    void expandKDNodeSubtree (SURFKDNode* kd_node, int flags);

    void assignPartKey (SURFKDNode* kd_node, int flags);

    double selectMedian (double* array, int n);

    double selectRank (double* array, int n, int r);

    void sortInsertion (double* array, int n);

    int partitionArray (double* array, int n, double pivot);

    void partitionFeatures (SURFKDNode* kd_node);

    int KDTreeBbfKnn (SURFKDNode* kd_root, SURFDescriptor* feat, int k,
                  SURFDescriptor*** nbrs, int max_nn_chks, int flags);

    MinPq* initMinPq ();

    int insertMinPq (MinPq* min_pq, void* data, int key);

    void decreasePqNodeKey (PqNode* pq_array, int i, int key);

    void restoreMinPqOrder (PqNode* pq_array, int i, int n);

    void* minPqExtractMin (MinPq* min_pq);

    SURFKDNode* exploreToLeaf (SURFKDNode* kd_node, SURFDescriptor* feat, MinPq* min_pq, int flags);

    double descrDistSq (SURFDescriptor* f1, SURFDescriptor* f2, int flags);

    int insertIntoNbrArray (SURFDescriptor* feat, SURFDescriptor** nbrs, int n, int k);

    void releaseMinPq (MinPq** min_pq);

    int doubleArray (void** array, int n, int size);

    void execute (CvSeq* seq);

    void transMatrix (CvSeq* seq);

    void Matrix ();

    void SVD ();

    int Jcbi (double A[], double V[]);

    void initSelect (CvSeq* seq);

    bool Marix (CvSeq* seq, double* RMatrix, double* TVector);

    void cutFalseMatrix();

    double calculateErr (CvSeq* seq, double RMatrix[9], double TVector[3]);

  public:
    QStringList files;
    int eff_match_pairs;
    std::vector<GlobalFeature> global_descriptors;  // Store global features
    std::vector< std::vector<SURFDescriptor> > surf_descriptors; // Store SURF features

  private:
    CvMat* wrap;
    IplImage* region_cache;
    IplImage* regions_cache[SCANOCTAVE * FILTERSCALE];

    CvMat* dx_cache;
    CvMat* dy_cache;
    CvMat* gauss_kernel_cache;

    double cos_cache[3600];
    double sin_cache[3600];

    /* Relate to the rotate translation vector */
    std::vector<pcl::PointXYZ> lib_points;  // 3D laser feature points of database
    std::vector<pcl::PointXYZ> sce_points;  // 3D laser feature points of scene

    double A[9];
    double A_AH[9];
    double AH_A[9];
    double U[9];    // Unit orthogonal eigenvectors matrix of AH*A
    double V[9];    // Unit orthogonal eigenvectors matrix of AH*A
    pcl::PointXYZ centroid1;  // The centroid of 3D laser feature point cloud in database
    pcl::PointXYZ centroid2;  // The centroid of 3D laser feature point cloud in scene

    double RMatrix[9];  // Rotation matrix
    double TVector[3];  // Translation vector

    std::vector<double*> RMatrixPlist;  // Pointer array of rotation matrix
    std::vector<double*> TVectorPlist;  // Pointer array of translation vector
    std::vector<bool> if_false_pairs;    // Whether false matching pairs
};

#endif // PLACE_RECOGNITION_H_