在vs中跑动ransac

期间遇到很多问题。

记一个最主要的是：

LINK2019 无法识别的外部符号，然后某一个函数的函数名 然后是 @@函数名 （@） 大概长成这样。或者还就根本就是 无法识别的外部符号。

解决方案：

我这里最主要的两个解决方案是：

2、你自己写的函数声明的头文件也写了函数定义的cpp文件，却依然出现LNK2019错误。可能原因：忘记将这两个文件加入工程了。一般出现于用Visual Studio和记事本（或UltraEdit）混合开发过程，你用记事本include了相应的头文件，却忘了在Visual Studio的工程中加入它们了。也可能出现于在解决方案的开发过程，在解决方案下的某个工程中加入了它们却忘了在其他工程中加入，我只接触过VC 6和VS 2008，中间好多年没用过新版本VS，到2008时突然发现怎么多了个“解决方案”，“解决方案”下面还可以放好多工程，于是经常在一个工程中写了共享的源代码，却忘了在别的工程中加入它们。这个问题类似于第1个，不同的是这个库是你自己提供的，但没有把它交给VS 2008编译出来。

3、你自己写的函数声明的头文件也写了函数定义的cpp文件也加入工程了而且你很确定函数体肯定是在这个库文件中，却依然出现LNK2019错误。可能原因：C语言和C++语言混编，因为C++支持函数重载所以C++编译器生成的库文件中的函数名会面目全非，例如C编译器会生成 _readRegmark 这个函数名，而C++编译器则生成了"void __cdecl readRegmark(char *)" (?readRegmark@@YAXPAD@Z)这么个函数名。当你的函数是用C语言写的，VS编译器会按C语言规则编译，但链接器却不知道还傻傻的用C++规则的函数名去找结果就找不到了，而你还百般肯定TM的不就在这个库中吗你个睁眼瞎。解决：在C语言的头文件中加入

其他扩展，

http://www.cnblogs.com/hiloves/p/4678848.html

【这个里面竟然不允许转载，太坑了。感谢这位同学的记录分享，十分受用】

【以前的那个 #define <iostream>  很坑，貌似这次存在的位置没有什么问题，http://www.cnblogs.com/letben/p/5459571.html 之前发现了这个问题，我忽然想到可能因为，某个 ifndef 这里可能会出这样的问题，顺序颠倒，可能就可以通过了。】

现在直接复制粘贴就能跑了。

然后这次需要一共13个文件，其中imgfeatures.h utils.h sift.h imgfeatures.c utils.c sift.c 这个跟之前的跑动sift是一样的【所以他们的地址：http://www.cnblogs.com/letben/p/5495353.html】。这里就不再粘贴出来了。除了mian.cpp 以外还有另外6个文件：分别是：kdtree.h minpq.h xform.h kdtree.c minpq.c xform.c

请看：

main.cpp

#include <iostream>//这个位置 貌似没那么严格
#include "opencv2/highgui/highgui.hpp"//这个是基本类库，高级图形用户接口，必须要引入
#include "opencv2/core/core.hpp"

#include "imgfeatures.h"
#include "kdtree.h"
#include "minpq.h"
#include "sift.h"
#include "utils.h"
#include "xform.h"

//extern "C"
//{
//#include "imgfeatures.h"
//#include "kdtree.h"
//#include "minpq.h"
//#include "sift.h"
//#include "utils.h"
//#include "xform.h"
//}

//在k-d树上进行BBF搜索的最大次数
/* the maximum number of keypoint NN candidates to check during BBF search */
#define KDTREE_BBF_MAX_NN_CHKS 200

//目标点与最近邻和次近邻的距离的比值的阈值，若大于此阈值，则剔除此匹配点对
//通常此值取0.6，值越小找到的匹配点对越精确，但匹配数目越少
/* threshold on squared ratio of distances between NN and 2nd NN */
//#define NN_SQ_DIST_RATIO_THR 0.49
#define NN_SQ_DIST_RATIO_THR 0.5

using namespace std;
using namespace cv;

void match(IplImage *img1, IplImage *img2)
{
    IplImage *img1_Feat = cvCloneImage(img1);//复制图1，深拷贝，用来画特征点
    IplImage *img2_Feat = cvCloneImage(img2);//复制图2，深拷贝，用来画特征点  

    struct feature *feat1, *feat2;//feat1：图1的特征点数组，feat2：图2的特征点数组
    int n1, n2;//n1:图1中的特征点个数，n2：图2中的特征点个数
    struct feature *feat;//每个特征点
    struct kd_node *kd_root;//k-d树的树根
    struct feature **nbrs;//当前特征点的最近邻点数组
    int matchNum;//经距离比值法筛选后的匹配点对的个数
    struct feature **inliers;//精RANSAC筛选后的内点数组
    int n_inliers;//经RANSAC算法筛选后的内点个数,即feat1中具有符合要求的特征点的个数  

    //默认提取的是LOWE格式的SIFT特征点
    //提取并显示第1幅图片上的特征点
    n1 = sift_features(img1, &feat1);//检测图1中的SIFT特征点,n1是图1的特征点个数
    export_features("feature1.txt", feat1, n1);//将特征向量数据写入到文件
    draw_features(img1_Feat, feat1, n1);//画出特征点
    cvShowImage("img1_Feat", img1_Feat);//显示  

    //提取并显示第2幅图片上的特征点
    n2 = sift_features(img2, &feat2);//检测图2中的SIFT特征点，n2是图2的特征点个数
    export_features("feature2.txt", feat2, n2);//将特征向量数据写入到文件
    draw_features(img2_Feat, feat2, n2);//画出特征点
    cvShowImage("img2_Feat", img2_Feat);//显示  

    Point pt1, pt2;//连线的两个端点
    double d0, d1;//feat1中每个特征点到最近邻和次近邻的距离
    matchNum = ;//经距离比值法筛选后的匹配点对的个数  

    IplImage* stacked;
    IplImage* stacked_ransac;
    //将2幅图片合成1幅图片,上下排列
    stacked = stack_imgs(img1, img2);//合成图像，显示经距离比值法筛选后的匹配结果
    stacked_ransac = stack_imgs(img1, img2);//合成图像，显示经RANSAC算法筛选后的匹配结果  

    //根据图2的特征点集feat2建立k-d树，返回k-d树根给kd_root
    kd_root = kdtree_build(feat2, n2);

    //遍历特征点集feat1，针对feat1中每个特征点feat，选取符合距离比值条件的匹配点，放到feat的fwd_match域中
    for (int i = ; i < n1; i++)
    {
        feat = feat1 + i;//第i个特征点的指针
        //在kd_root中搜索目标点feat的2个最近邻点，存放在nbrs中，返回实际找到的近邻点个数
        int k = kdtree_bbf_knn(kd_root, feat, , &nbrs, KDTREE_BBF_MAX_NN_CHKS);
        if (k == )
        {
            d0 = descr_dist_sq(feat, nbrs[]);//feat与最近邻点的距离的平方
            d1 = descr_dist_sq(feat, nbrs[]);//feat与次近邻点的距离的平方
            //若d0和d1的比值小于阈值NN_SQ_DIST_RATIO_THR，则接受此匹配，否则剔除
            if (d0 < d1 * NN_SQ_DIST_RATIO_THR)
            {   //将目标点feat和最近邻点作为匹配点对
                pt1 = Point(cvRound(feat->x), cvRound(feat->y));//图1中点的坐标
                pt2 = Point(cvRound(nbrs[]->x), cvRound(nbrs[]->y));//图2中点的坐标(feat的最近邻点)
                pt2.y += img1->height;//由于两幅图是上下排列的，pt2的纵坐标加上图1的高度，作为连线的终点
                cvLine(stacked, pt1, pt2, CV_RGB(, , ), , , );//画出连线
                matchNum++;//统计匹配点对的个数
                feat1[i].fwd_match = nbrs[];//使点feat的fwd_match域指向其对应的匹配点
            }
        }
        free(nbrs);//释放近邻数组
    }
    //qDebug() << tr("经距离比值法筛选后的匹配点对个数：") << matchNum << endl;
    cout << "经距离比值法筛选后的匹配点对个数：" << matchNum << endl;
    //显示并保存经距离比值法筛选后的匹配图
    cvNamedWindow("IMG_MATCH1",);//创建窗口
    cvShowImage("IMG_MATCH1", stacked);//显示  

    //利用RANSAC算法筛选匹配点,计算变换矩阵H
    CvMat * H = ransac_xform(feat1, n1, FEATURE_FWD_MATCH, lsq_homog, , 0.01, homog_xfer_err, 3.0, &inliers, &n_inliers);
    //qDebug() << tr("经RANSAC算法筛选后的匹配点对个数：") << n_inliers << endl;
    cout << "经RANSAC算法筛选后的匹配点对个数：" << matchNum << endl;
    //遍历经RANSAC算法筛选后的特征点集合inliers，找到每个特征点的匹配点，画出连线
    for (int i = ; i<n_inliers; i++)
    {
        feat = inliers[i];//第i个特征点
        pt1 = Point(cvRound(feat->x), cvRound(feat->y));//图1中点的坐标
        pt2 = Point(cvRound(feat->fwd_match->x), cvRound(feat->fwd_match->y));//图2中点的坐标(feat的匹配点)
        //qDebug() << "(" << pt1.x << "," << pt1.y << ")--->(" << pt2.x << "," << pt2.y << ")" << endl;
        cout << "(" << pt1.x << "," << pt1.y << ")--->(" << pt2.x << "," << pt2.y << ")" << endl;
        pt2.y += img1->height;//由于两幅图是上下排列的，pt2的纵坐标加上图1的高度，作为连线的终点
        cvLine(stacked_ransac, pt1, pt2, CV_RGB(, , ), , , );//画出连线
    }
    cvNamedWindow("IMG_MATCH2",);//创建窗口
    cvShowImage("IMG_MATCH2", stacked_ransac);//显示
}

int main(){

    IplImage * img1 = cvLoadImage("l2.png");
    IplImage * img2 = cvLoadImage("r2.png");

    match(img1, img2);

    cvWaitKey();

}

minpq.h

/**@file
Functions and structures for implementing a minimizing priority queue.

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>
@version 1.1.2-20100521
*/

/*
此文件中是最小优先队列(小顶堆)相关的一些结构和函数声明
*/

#ifndef MINPQ_H
#define MINPQ_H

#include <stdlib.h>

/******************************* Defs and macros *****************************/

/* initial # of priority queue elements for which to allocate space */
#define MINPQ_INIT_NALLOCD 512  //初始分配空间个数

#ifdef __cplusplus
extern "C" {
#endif

    /********************************** Structures *******************************/
    /*结点结构*/
    /** an element in a minimizing priority queue */
    struct pq_node
    {
        void* data;
        int key;
    };

    /*最小优先队列结构*/
    /** a minimizing priority queue */
    struct min_pq
    {
        struct pq_node* pq_array;    /* array containing priority queue */ //结点指针
        int nallocd;                 /* number of elements allocated */ //分配的空间个数
        int n;                       /**< number of elements in pq */ //元素个数
    };

    /*************************** Function Prototypes *****************************/
    /*初始化最小优先级队列
    */
    /**
    Creates a new minimizing priority queue.
    */
    extern struct min_pq* minpq_init();

    /*插入元素到优先队列
    参数：
    min_pq：优先队列
    data:要插入的数据
    key:与data关联的键值
    返回值：0：成功，1：失败
    */
    /**
    Inserts an element into a minimizing priority queue.

    @param min_pq a minimizing priority queue
    @param data the data to be inserted
    @param key the key to be associated with \a data

    @return Returns 0 on success or 1 on failure.
    */
    extern int minpq_insert(struct min_pq* min_pq, void* data, int key);

    /*返回优先队列中键值最小的元素，但并不删除它
    参数：min_pq：优先队列
    返回值：最小元素的指针
    */
    /**
    Returns the element of a minimizing priority queue with the smallest key
    without removing it from the queue.
    @param min_pq a minimizing priority queue
    @return Returns the element of \a min_pq with the smallest key or NULL if \a min_pq is empty
    */
    extern void* minpq_get_min(struct min_pq* min_pq);

    /*返回并移除具有最小键值的元素
    参数：min_pq：优先级队列
    返回值：最小元素的指针
    */
    /**
    Removes and returns the element of a minimizing priority queue with the smallest key.
    @param min_pq a minimizing priority queue
    @return Returns the element of \a min_pq with the smallest key of NULL if \a min_pq is empty
    */
    extern void* minpq_extract_min(struct min_pq* min_pq);

    /*释放优先队列
    */
    /**
    De-allocates the memory held by a minimizing priorioty queue
    @param min_pq pointer to a minimizing priority queue
    */
    extern void minpq_release(struct min_pq** min_pq);

#ifdef __cplusplus
}
#endif

#endif

kdtree.h

/**@file
Functions and structures for maintaining a k-d tree database of image
features.

For more information, refer to:

Beis, J. S. and Lowe, D. G.  Shape indexing using approximate
nearest-neighbor search in high-dimensional spaces.  In <EM>Conference
on Computer Vision and Pattern Recognition (CVPR)</EM> (2003),
pp. 1000--1006.

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>

@version 1.1.2-20100521
*/

/*
此文件中包含K-D树的建立与搜索函数的声明
*/

#ifndef KDTREE_H
#define KDTREE_H

#include "opencv\cxcore.h"

#ifdef __cplusplus
extern "C" {
#endif

    /********************************* Structures ********************************/

    struct feature;

    /*K-D树中的结点结构*/
    /** a node in a k-d tree */
    struct kd_node
    {
        int ki;                      /**< partition key index */ //分割位置(枢轴)的维数索引(哪一维是分割位置)，取值为1-128
        double kv;                   /**< partition key value */  //枢轴的值(所有特征向量在枢轴索引维数上的分量的中值)
        int leaf;                    /**< 1 if node is a leaf, 0 otherwise */ //是否叶子结点的标志
        struct feature* features;    /**< features at this node */  //此结点对应的特征点集合(数组)
        int n;                       /**< number of features */ //特征点的个数
        struct kd_node* kd_left;     /**< left child */  //左子树
        struct kd_node* kd_right;    /**< right child */  //右子树
    };

    /*************************** Function Prototypes *****************************/
    /*根据给定的特征点集合建立k-d树
    参数：
    features：特征点数组，注意：此函数将会改变features数组中元素的排列顺序
    n：特征点个数
    返回值：建立好的k-d树的树根指针
    */
    /**
    A function to build a k-d tree database from keypoints in an array.

    @param features an array of features; <EM>this function rearranges the order
    of the features in this array, so you should take appropriate measures
    if you are relying on the order of the features (e.g. call this function
    before order is important)</EM>
    @param n the number of features in \a features
    @return Returns the root of a kd tree built from \a features.
    */
    extern struct kd_node* kdtree_build(struct feature* features, int n);

    /*用BBF算法在k-d树中查找指定特征点的k个最近邻特征点
    参数：
    kd_root：图像特征的k-d树的树根
    feat：目标特征点
    k：近邻个数
    nbrs：k个近邻特征点的指针数组，按到目标特征点的距离升序排列
    此数组的内存将在本函数中被分配，使用完后必须在调用出释放：free(*nbrs)
    max_nn_chks：搜索的最大次数，超过此值不再搜索
    返回值：存储在nbrs中的近邻个数，返回-1表示失败
    */
    /**
    Finds an image feature's approximate k nearest neighbors in a kd tree using
    Best Bin First search.

    @param kd_root root of an image feature kd tree
    @param feat image feature for whose neighbors to search
    @param k number of neighbors to find
    @param nbrs pointer to an array in which to store pointers to neighbors
    in order of increasing descriptor distance; memory for this array is
    allocated by this function and must be freed by the caller using
    free(*nbrs)
    @param max_nn_chks search is cut off after examining this many tree entries

    @return Returns the number of neighbors found and stored in \a nbrs, or
    -1 on error.
    */
    extern int kdtree_bbf_knn(struct kd_node* kd_root, struct feature* feat,
        int k, struct feature*** nbrs, int max_nn_chks);

    /**
    Finds an image feature's approximate k nearest neighbors within a specified
    spatial region in a kd tree using Best Bin First search.

    @param kd_root root of an image feature kd tree
    @param feat image feature for whose neighbors to search
    @param k number of neighbors to find
    @param nbrs pointer to an array in which to store pointers to neighbors
    in order of increasing descriptor distance; memory for this array is
    allocated by this function and must be freed by the caller using
    free(*nbrs)
    @param max_nn_chks search is cut off after examining this many tree entries
    @param rect rectangular region in which to search for neighbors
    @param model if true, spatial search is based on kdtree features' model
    locations; otherwise it is based on their image locations

    @return Returns the number of neighbors found and stored in \a nbrs
    (in case \a k neighbors could not be found before examining
    \a max_nn_checks keypoint entries).
    */
    extern int kdtree_bbf_spatial_knn(struct kd_node* kd_root,
    struct feature* feat, int k,
    struct feature*** nbrs, int max_nn_chks,
        CvRect rect, int model);

    /*释放k-d树占用的存储空间
    */
    /**
    De-allocates memory held by a kd tree

    @param kd_root pointer to the root of a kd tree
    */
    extern void kdtree_release(struct kd_node* kd_root);
#ifdef __cplusplus
}
#endif

#endif

xform.h

/**@file
Functions for computing transforms from image feature correspondences

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>
@version 1.1.2-20100521
*/

#ifndef XFORM_H
#define XFORM_H

#include <cxcore.h>

#ifdef __cplusplus
extern "C" {
#endif

    /********************************** Structures *******************************/

    struct feature;

    /** holds feature data relevant to ransac */
    struct ransac_data
    {
        void* orig_feat_data;
        int sampled;
    };

    /******************************* Defs and macros *****************************/

    /* RANSAC error tolerance in pixels */
#define RANSAC_ERR_TOL 3

    /** pessimistic estimate of fraction of inlers for RANSAC */
#define RANSAC_INLIER_FRAC_EST 0.25

    /** estimate of the probability that a correspondence supports a bad model */
#define RANSAC_PROB_BAD_SUPP 0.10

    /* extracts a feature's RANSAC data */
#define feat_ransac_data( feat ) ( (struct ransac_data*) (feat)->feature_data )

    /**
    Prototype for transformation functions passed to ransac_xform().  Functions
    of this type should compute a transformation matrix given a set of point
    correspondences.

    @param pts array of points
    @param mpts array of corresponding points; each \a pts[\a i], \a i=0..\a n-1,
    corresponds to \a mpts[\a i]
    @param n number of points in both \a pts and \a mpts

    @return Should return a transformation matrix that transforms each point in
    \a pts to the corresponding point in \a mpts or NULL on failure.
    */
    typedef CvMat* (*ransac_xform_fn)(CvPoint2D64f* pts, CvPoint2D64f* mpts,
        int n);

    /**
    Prototype for error functions passed to ransac_xform().  For a given
    point, its correspondence, and a transform, functions of this type should
    compute a measure of error between the correspondence and the point after
    the point has been transformed by the transform.

    @param pt a point
    @param mpt \a pt's correspondence
    @param T a transform

    @return Should return a measure of error between \a mpt and \a pt after
    \a pt has been transformed by the transform \a T.
    */
    typedef double(*ransac_err_fn)(CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* M);

    /***************************** Function Prototypes ***************************/

    /**
    Calculates a best-fit image transform from image feature correspondences
    using RANSAC.

    For more information refer to:

    Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
    model fitting with applications to image analysis and automated cartography.
    <EM>Communications of the ACM, 24</EM>, 6 (1981), pp. 381--395.

    @param features an array of features; only features with a non-NULL match
    of type \a mtype are used in homography computation
    @param n number of features in \a feat
    @param mtype determines which of each feature's match fields to use
    for transform computation; should be one of FEATURE_FWD_MATCH,
    FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
    correspondences are assumed to be between a feature's img_pt field
    and its match's mdl_pt field, otherwise correspondences are assumed to
    be between the the feature's img_pt field and its match's img_pt field
    @param xform_fn pointer to the function used to compute the desired
    transformation from feature correspondences
    @param m minimum number of correspondences necessary to instantiate the
    transform computed by \a xform_fn
    @param p_badxform desired probability that the final transformation
    returned by RANSAC is corrupted by outliers (i.e. the probability that
    no samples of all inliers were drawn)
    @param err_fn pointer to the function used to compute a measure of error
    between putative correspondences for a given transform
    @param err_tol correspondences within this distance of each other are
    considered as inliers for a given transform
    @param inliers if not NULL, output as an array of pointers to the final
    set of inliers; memory for this array is allocated by this function and
    must be freed by the caller using free(*inliers)
    @param n_in if not NULL, output as the final number of inliers

    @return Returns a transformation matrix computed using RANSAC or NULL
    on error or if an acceptable transform could not be computed.
    */
    extern CvMat* ransac_xform(struct feature* features, int n, int mtype,
        ransac_xform_fn xform_fn, int m,
        double p_badxform, ransac_err_fn err_fn,
        double err_tol, struct feature*** inliers,
        int* n_in);

    /**
    Calculates a planar homography from point correspondeces using the direct
    linear transform.  Intended for use as a ransac_xform_fn.

    @param pts array of points
    @param mpts array of corresponding points; each \a pts[\a i], \a i=0..\a
    n-1, corresponds to \a mpts[\a i]
    @param n number of points in both \a pts and \a mpts; must be at least 4

    @return Returns the \f$3 \times 3\f$ planar homography matrix that
    transforms points in \a pts to their corresponding points in \a mpts
    or NULL if fewer than 4 correspondences were provided
    */
    extern CvMat* dlt_homog(CvPoint2D64f* pts, CvPoint2D64f* mpts, int n);

    /**
    Calculates a least-squares planar homography from point correspondeces.
    Intended for use as a ransac_xform_fn.

    @param pts array of points
    @param mpts array of corresponding points; each \a pts[\a i], \a i=0..\a n-1,
    corresponds to \a mpts[\a i]
    @param n number of points in both \a pts and \a mpts; must be at least 4

    @return Returns the \f$3 \times 3\f$ least-squares planar homography
    matrix that transforms points in \a pts to their corresponding points
    in \a mpts or NULL if fewer than 4 correspondences were provided
    */
    extern CvMat* lsq_homog(CvPoint2D64f* pts, CvPoint2D64f* mpts, int n);

    /**
    Calculates the transfer error between a point and its correspondence for
    a given homography, i.e. for a point \f$x\f$, it's correspondence \f$x'\f$,
    and homography \f$H\f$, computes \f$d(x', Hx)^2\f$.  Intended for use as a
    ransac_err_fn.

    @param pt a point
    @param mpt \a pt's correspondence
    @param H a homography matrix

    @return Returns the transfer error between \a pt and \a mpt given \a H
    */
    extern double homog_xfer_err(CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H);

    /**
    Performs a perspective transformation on a single point.  That is, for a
    point \f$(x, y)\f$ and a \f$3 \times 3\f$ matrix \f$T\f$ this function
    returns the point \f$(u, v)\f$, where<BR>

    \f$[x' \ y' \ w']^T = T \times [x \ y \ 1]^T\f$,<BR>

    and<BR>

    \f$(u, v) = (x'/w', y'/w')\f$.

    Note that affine transforms are a subset of perspective transforms.

    @param pt a 2D point
    @param T a perspective transformation matrix

    @return Returns the point \f$(u, v)\f$ as above.
    */
    extern CvPoint2D64f persp_xform_pt(CvPoint2D64f pt, CvMat* T);

#ifdef __cplusplus
}
#endif

#endif

kdtree.c

/*
Functions and structures for maintaining a k-d tree database of image
features.

For more information, refer to:

Beis, J. S. and Lowe, D. G.  Shape indexing using approximate
nearest-neighbor search in high-dimensional spaces.  In <EM>Conference
on Computer Vision and Pattern Recognition (CVPR)</EM> (2003),
pp. 1000--1006.

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>

@version 1.1.2-20100521
*/

/*
此文件中有k-d树的建立和BBF查找函数的实现
*/

#include "kdtree.h"
#include "minpq.h"
#include "imgfeatures.h"
#include "utils.h"

#include "opencv\cxcore.h"

#include <stdio.h>

//BBF中用到的结构，可存储当前点到目标点的距离
//在kd树搜索过程中，此类型数据会被赋值给feature结构的feature_data成员
struct bbf_data
{
    double d;  //此特征点到目标点的欧式距离值
    void* old_data; //保存此特征点的feature_data域的以前的值
};

/************************ 未暴露接口的一些本地函数的声明 **************************/
/************************* Local Function Prototypes *************************/

//用给定的特征点集初始化k-d树节点
static struct kd_node* kd_node_init(struct feature*, int);
//扩展指定的k-d树节点及其左右孩子
static void expand_kd_node_subtree(struct kd_node*);
//确定输入节点的枢轴索引和枢轴值
static void assign_part_key(struct kd_node*);
//找到输入数组的中值
static double median_select(double*, int);
//找到输入数组中第r小的数
static double rank_select(double*, int, int);
//用插入法对输入数组进行升序排序
static void insertion_sort(double*, int);
//根据给定的枢轴值分割数组，使数组前部分小于pivot，后部分大于pivot
static int partition_array(double*, int, double);
//在指定的k-d树节点上划分特征点集
static void partition_features(struct kd_node*);
//从给定结点搜索k-d树直到叶节点，搜索过程中将未搜索的节点根据优先级放入队列
static struct kd_node* explore_to_leaf(struct kd_node*, struct feature*, struct min_pq*);
//插入一个特征点到最近邻数组，使数组中的点按到目标点的距离升序排列
static int insert_into_nbr_array(struct feature*, struct feature**, int, int);
//判断给定点是否在某矩形中
static int within_rect(CvPoint2D64f, CvRect);
//将两点连接成直线 

/******************** 已在kdtree.h中声明的函数 **********************/
/******************** Functions prototyped in kdtree.h **********************/

/*根据给定的特征点集合建立k-d树
参数：
features：特征点数组，注意：此函数将会改变features数组中元素的排列顺序
n：特征点个数
返回值：建立好的k-d树的树根指针
*/
/*
A function to build a k-d tree database from keypoints in an array.

@param features an array of features
@param n the number of features in features

@return Returns the root of a kd tree built from features or NULL on error.
*/
struct kd_node* kdtree_build(struct feature* features, int n)
{
    struct kd_node* kd_root;

    //输入参数检查
    if (!features || n <= )
    {
        fprintf(stderr, "Warning: kdtree_build(): no features, %s, line %d\n",
            __FILE__, __LINE__);
        return NULL;
    }

    //调用函数，用给定的特征点集初始化k-d树节点，返回值作为树根
    kd_root = kd_node_init(features, n);
    //调用函数，扩展根节点kd_root及其左右孩子
    expand_kd_node_subtree(kd_root);

    return kd_root;
}

/*用BBF算法在k-d树中查找指定特征点的k个最近邻特征点
参数：
kd_root：图像特征的k-d树的树根
feat：目标特征点
k：近邻个数
nbrs：k个近邻特征点的指针数组，按到目标特征点的距离升序排列
此数组的内存将在本函数中被分配，使用完后必须在调用出释放：free(*nbrs)
max_nn_chks：搜索的最大次数，超过此值不再搜索
返回值：存储在nbrs中的近邻个数，返回-1表示失败
*/
/*
Finds an image feature's approximate k nearest neighbors in a kd tree using
Best Bin First search.
@param kd_root root of an image feature kd tree
@param feat image feature for whose neighbors to search
@param k number of neighbors to find
@param nbrs pointer to an array in which to store pointers to neighbors
in order of increasing descriptor distance
@param max_nn_chks search is cut off after examining this many tree entries
@return Returns the number of neighbors found and stored in nbrs, or -1 on error.
*/
int kdtree_bbf_knn(struct kd_node* kd_root, struct feature* feat, int k,
struct feature*** nbrs, int max_nn_chks)
{
    struct kd_node* expl; //expl是当前搜索节点
    struct min_pq* min_pq; //优先级队列
    struct feature* tree_feat, ** _nbrs; //tree_feat是单个SIFT特征，_nbrs中存放着查找出来的近邻特征节点
    struct bbf_data* bbf_data; //bbf_data是一个用来存放临时特征数据和特征间距离的缓存结构
    int i, t = , n = ; //t是搜索的最大次数，n是当前最近邻数组中的元素个数

    //输入参数检查
    if (!nbrs || !feat || !kd_root)
    {
        fprintf(stderr, "Warning: NULL pointer error, %s, line %d\n", __FILE__, __LINE__);
        return -;
    }

    _nbrs = calloc(k, sizeof(struct feature*)); //给查找结果分配相应大小的内存
    min_pq = minpq_init(); //min_pq队列初始化，分配默认大小的空间
    minpq_insert(min_pq, kd_root, ); //将根节点先插入到min_pq优先级队列中

    //min_pq队列没有回溯完且未达到搜索最大次数
    while (min_pq->n >  && t < max_nn_chks)
    {
        //从min_pq中提取(并移除)优先级最高的节点，赋值给当前节点expl
        expl = (struct kd_node*)minpq_extract_min(min_pq);
        if (!expl)
        {   //出错处理
            fprintf(stderr, "Warning: PQ unexpectedly empty, %s line %d\n", __FILE__, __LINE__);
            goto fail;
        }
        //从当前搜索节点expl一直搜索到叶子节点，搜索过程中将未搜索的节点根据优先级放入队列，返回值为叶子节点
        expl = explore_to_leaf(expl, feat, min_pq);
        if (!expl)
        {   //出错处理
            fprintf(stderr, "Warning: PQ unexpectedly empty, %s line %d\n", __FILE__, __LINE__);
            goto fail;
        }

        //比较查找最近邻
        for (i = ; i < expl->n; i++)
        {
            tree_feat = &expl->features[i];//第i个特征点的指针
            bbf_data = malloc(sizeof(struct bbf_data));//新建bbf结构
            if (!bbf_data)
            {   //出错处理
                fprintf(stderr, "Warning: unable to allocate memory," " %s line %d\n", __FILE__, __LINE__);
                goto fail;
            }
            bbf_data->old_data = tree_feat->feature_data;//保存第i个特征点的feature_data域以前的值
            bbf_data->d = descr_dist_sq(feat, tree_feat);//当前搜索点和目标点之间的欧氏距离
            tree_feat->feature_data = bbf_data;//将bbf结构赋给此特征点的feature_data域
            //判断并插入符合条件的特征点到最近邻数组_nbrs中,插入成功返回1
            //当最近邻数组中元素个数已达到k时，继续插入元素个数不会增加，但会更新元素的值
            n += insert_into_nbr_array(tree_feat, _nbrs, n, k);
        }
        t++;//搜索次数
    }

    minpq_release(&min_pq);//释放优先队列

    //对于最近邻数组中的特征点，恢复其feature_data域的值
    for (i = ; i < n; i++)
    {
        bbf_data = _nbrs[i]->feature_data;
        _nbrs[i]->feature_data = bbf_data->old_data;//将之前的数据赋值给feature_data域
        free(bbf_data);
    }
    *nbrs = _nbrs;
    return n;

    //失败处理
fail:
    minpq_release(&min_pq);
    //对于最近邻数组中的特征点，恢复其feature_data域的值
    for (i = ; i < n; i++)
    {
        bbf_data = _nbrs[i]->feature_data;
        _nbrs[i]->feature_data = bbf_data->old_data;
        free(bbf_data);
    }
    free(_nbrs);
    *nbrs = NULL;
    return -;
}

/*
Finds an image feature's approximate k nearest neighbors within a specified
spatial region in a kd tree using Best Bin First search.

@param kd_root root of an image feature kd tree
@param feat image feature for whose neighbors to search
@param k number of neighbors to find
@param nbrs pointer to an array in which to store pointers to neighbors
in order of increasing descriptor distance
@param max_nn_chks search is cut off after examining this many tree entries
@param rect rectangular region in which to search for neighbors
@param model if true, spatial search is based on kdtree features' model
locations; otherwise it is based on their image locations

@return Returns the number of neighbors found and stored in \a nbrs
(in case \a k neighbors could not be found before examining
\a max_nn_checks keypoint entries).
*/
int kdtree_bbf_spatial_knn(struct kd_node* kd_root, struct feature* feat,
    int k, struct feature*** nbrs, int max_nn_chks,
    CvRect rect, int model)
{
    struct feature** all_nbrs, ** sp_nbrs;
    CvPoint2D64f pt;
    int i, n, t = ;

    n = kdtree_bbf_knn(kd_root, feat, max_nn_chks, &all_nbrs, max_nn_chks);
    sp_nbrs = calloc(k, sizeof(struct feature*));
    for (i = ; i < n; i++)
    {
        if (model)
            pt = all_nbrs[i]->mdl_pt;
        else
            pt = all_nbrs[i]->img_pt;

        if (within_rect(pt, rect))
        {
            sp_nbrs[t++] = all_nbrs[i];
            if (t == k)
                goto end;
        }
    }
end:
    free(all_nbrs);
    *nbrs = sp_nbrs;
    return t;
}

/*释放k-d树占用的存储空间
*/
/*
De-allocates memory held by a kd tree

@param kd_root pointer to the root of a kd tree
*/
void kdtree_release(struct kd_node* kd_root)
{
    if (!kd_root)
        return;
    kdtree_release(kd_root->kd_left);
    kdtree_release(kd_root->kd_right);
    free(kd_root);
}

/************************ 未暴露接口的一些本地函数 **************************/
/************************ Functions prototyped here **************************/

/*用给定的特征点集初始化k-d树节点
参数：
features：特征点集
n：特征点个数
返回值：k-d树节点指针
*/
/*
Initializes a kd tree node with a set of features.  The node is not
expanded, and no ordering is imposed on the features.

@param features an array of image features
@param n number of features

@return Returns an unexpanded kd-tree node.
*/
static struct kd_node* kd_node_init(struct feature* features, int n)
{
    struct kd_node* kd_node;

    kd_node = malloc(sizeof(struct kd_node));//分配内存
    memset(kd_node, , sizeof(struct kd_node));
    kd_node->ki = -;//枢轴索引
    kd_node->features = features;//节点对应的特征点集
    kd_node->n = n;//特征点的个数

    return kd_node;
}

/*递归的扩展指定的k-d树节点及其左右孩子
*/
/*
Recursively expands a specified kd tree node into a tree whose leaves
contain one entry each.

@param kd_node an unexpanded node in a kd tree
*/
static void expand_kd_node_subtree(struct kd_node* kd_node)
{
    //基本情况：叶子节点
    /* base case: leaf node */
    if (kd_node->n ==  || kd_node->n == )
    {
        kd_node->leaf = ;//叶节点标志位设为1
        return;
    }

    //调用函数，确定节点的枢轴索引和枢轴值
    assign_part_key(kd_node);
    //在指定k-d树节点上划分特征点集(即根据指定节点的ki和kv值来划分特征点集)
    partition_features(kd_node);

    //继续扩展左右孩子
    if (kd_node->kd_left)
        expand_kd_node_subtree(kd_node->kd_left);
    if (kd_node->kd_right)
        expand_kd_node_subtree(kd_node->kd_right);
}

/*确定输入节点的枢轴索引和枢轴值
参数：kd_node：输入的k-d树节点
函数执行完后将给kd_node的ki和kv成员复制
*/
/*
Determines the descriptor index at which and the value with which to
partition a kd tree node's features.

@param kd_node a kd tree node
*/
static void assign_part_key(struct kd_node* kd_node)
{
    struct feature* features;
    //枢轴的值kv，均值mean，方差var，方差最大值var_max
    double kv, x, mean, var, var_max = ;
    double* tmp;
    int d, n, i, j, ki = ; //枢轴索引ki

    features = kd_node->features;
    n = kd_node->n;//结点个数
    d = features[].d;//特征向量的维数

    //枢轴的索引值就是方差最大的那一维的维数,即n个128维的特征向量中，若第k维的方差最大，则k就是枢轴(分割位置)
    /* partition key index is that along which descriptors have most variance */
    for (j = ; j < d; j++)
    {
        mean = var = ;
        //求第j维的均值
        for (i = ; i < n; i++)
            mean += features[i].descr[j];
        mean /= n;
        //求第j维的方差
        for (i = ; i < n; i++)
        {
            x = features[i].descr[j] - mean;
            var += x * x;
        }
        var /= n;
        //找到最大方差的维数
        if (var > var_max)
        {
            ki = j;//最大方差的维数就是枢轴
            var_max = var;
        }
    }

    //枢轴的值就是所有特征向量的ki维的中值(按ki维排序后中间的那个值)
    /* partition key value is median of descriptor values at ki */
    tmp = calloc(n, sizeof(double));
    for (i = ; i < n; i++)
        tmp[i] = features[i].descr[ki];
    //调用函数，找tmp数组的中值
    kv = median_select(tmp, n);
    free(tmp);

    kd_node->ki = ki;//枢轴的维数索引
    kd_node->kv = kv;//枢轴的值
}

/*找到输入数组的中值
参数：
array：输入数组，元素顺序将会被改动
n：元素个数
返回值：中值
*/
/*
Finds the median value of an array.  The array's elements are re-ordered
by this function.

@param array an array; the order of its elelemts is reordered
@param n number of elements in array

@return Returns the median value of array.
*/
static double median_select(double* array, int n)
{
    //调用函数，找array数组中的第(n-1)/2小的数，即中值
    return rank_select(array, n, (n - ) / );
}

/*找到输入数组中第r小的数
参数：
array：输入数组，元素顺序将会被改动
n：元素个数
r：找第r小元素
返回值：第r小的元素值
*/
/*
Finds the element of a specified rank in an array using the linear time
median-of-medians algorithm by Blum, Floyd, Pratt, Rivest, and Tarjan.
The elements of the array are re-ordered by this function.
@param array an array; the order of its elelemts is reordered
@param n number of elements in array
@param r the zero-based rank of the element to be selected
@return Returns the element from array with zero-based rank r.
*/
static double rank_select(double* array, int n, int r)
{
    double* tmp, med;
    int gr_5, gr_tot, rem_elts, i, j;

    /* base case */
    if (n == )
        return array[];

    //将数组分成5个一组，共gr_tot组
    /* divide array into groups of 5 and sort them */
    gr_5 = n / ; //组的个数-1，n/5向下取整
    gr_tot = cvCeil(n / 5.0); //组的个数，n/5向上取整
    rem_elts = n % ;//最后一组中的元素个数
    tmp = array;
    //对每组进行插入排序
    for (i = ; i < gr_5; i++)
    {
        insertion_sort(tmp, );
        tmp += ;
    }
    //最后一组
    insertion_sort(tmp, rem_elts);

    //找中值的中值
    /* recursively find the median of the medians of the groups of 5 */
    tmp = calloc(gr_tot, sizeof(double));
    //将每个5元组中的中值(即下标为2,2+5,...的元素)复制到temp数组
    for (i = , j = ; i < gr_5; i++, j += )
        tmp[i] = array[j];
    //最后一组的中值
    if (rem_elts)
        tmp[i++] = array[n -  - rem_elts / ];
    //找temp中的中值med，即中值的中值
    med = rank_select(tmp, i, (i - ) / );
    free(tmp);

    //利用中值的中值划分数组，看划分结果是否是第r小的数，若不是则递归调用rank_select重新选择
    /* partition around median of medians and recursively select if necessary */
    j = partition_array(array, n, med);//划分数组，返回med在新数组中的索引
    if (r == j)//结果是第r小的数
        return med;
    else if (r < j)//第r小的数在前半部分
        return rank_select(array, j, r);
    else//第r小的数在后半部分
    {
        array += j + ;
        return rank_select(array, (n - j - ), (r - j - ));
    }
}

/*用插入法对输入数组进行升序排序
参数：
array：输入数组
n：元素个数
*/
/*
Sorts an array in place into increasing order using insertion sort.

@param array an array
@param n number of elements
*/
static void insertion_sort(double* array, int n)
{
    double k;
    int i, j;

    for (i = ; i < n; i++)
    {
        k = array[i];
        j = i - ;
        while (j >=  && array[j] > k)
        {
            array[j + ] = array[j];
            j -= ;
        }
        array[j + ] = k;
    }
}

/*根据给定的枢轴值分割数组，使数组前部分小于pivot，后部分大于pivot
参数：
array：输入数组
n：数组的元素个数
pivot：枢轴值
返回值：分割后枢轴的下标
*/
/*
Partitions an array around a specified value.
@param array an array
@param n number of elements
@param pivot value around which to partition
@return Returns index of the pivot after partitioning
*/
static int partition_array(double* array, int n, double pivot)
{
    double tmp;
    int p, i, j;

    i = -;
    for (j = ; j < n; j++)
    if (array[j] <= pivot)
    {
        tmp = array[++i];
        array[i] = array[j];
        array[j] = tmp;
        if (array[i] == pivot)
            p = i;//p保存枢轴的下标
    }
    //将枢轴和最后一个小于枢轴的数对换
    array[p] = array[i];
    array[i] = pivot;

    return i;
}

/*在指定的k-d树节点上划分特征点集
使得特征点集的前半部分是第ki维小于枢轴的点，后半部分是第ki维大于枢轴的点
*/
/*
Partitions the features at a specified kd tree node to create its two
children.

@param kd_node a kd tree node whose partition key is set
*/
static void partition_features(struct kd_node* kd_node)
{
    struct feature* features, tmp;
    double kv;
    int n, ki, p, i, j = -;

    features = kd_node->features;//特征点数组
    n = kd_node->n;//特征点个数
    //printf("%d\n",n);
    ki = kd_node->ki;//枢轴的维数索引(哪一维是枢轴)
    kv = kd_node->kv;//枢轴的值
    for (i = ; i < n; i++)
    {
        //若第i个特征点的特征向量的第ki维的值小于kv
        if (features[i].descr[ki] <= kv)
        {
            tmp = features[++j];
            features[j] = features[i];
            features[i] = tmp;
            if (features[j].descr[ki] == kv)
                p = j;//p保存枢轴所在的位置
        }
    }
    //将枢轴features[p]和最后一个小于枢轴的点features[j]对换
    tmp = features[p];
    features[p] = features[j];
    features[j] = tmp;
    //此后，枢轴的位置下标为j

    //若所有特征点落在同一侧，则此节点成为叶节点
    /* if all records fall on same side of partition, make node a leaf */
    if (j == n - )
    {
        kd_node->leaf = ;
        return;
    }

    //初始化左孩子的根节点，左孩子共j+1个特征点
    kd_node->kd_left = kd_node_init(features, j + );
    //初始化右孩子的根节点，右孩子共n-j-1个特征点
    kd_node->kd_right = kd_node_init(features + (j + ), (n - j - ));
}

/*从给定结点搜索k-d树直到叶节点，搜索过程中将未搜索的节点根据优先级放入队列
优先级队列和搜索路径是同时生成的，这也是BBF算法的精髓所在：在二叉搜索的时
候将搜索路径另一侧的分支加入到优先级队列中，供回溯时查找。而优先级队列的排
序就是根据目标特征与分割超平面的距离ABS(kv - feat->descr[ki])
参数：
kd_node：要搜索的子树的树根
feat：目标特征点
min_pq：优先级队列
返回值：叶子节点的指针
*/
/*
Explores a kd tree from a given node to a leaf.  Branching decisions are
made at each node based on the descriptor of a given feature.  Each node
examined but not explored is put into a priority queue to be explored
later, keyed based on the distance from its partition key value to the
given feature's desctiptor.
@param kd_node root of the subtree to be explored
@param feat feature upon which branching decisions are based
@param min_pq a minimizing priority queue into which tree nodes are placed as described above
@return Returns a pointer to the leaf node at which exploration ends or
NULL on error.
*/
static struct kd_node* explore_to_leaf(struct kd_node* kd_node, struct feature* feat,
struct min_pq* min_pq)
{
    //unexpl中存放着优先级队列的候选节点(还未搜索的节点)，expl为当前搜索节点
    struct kd_node* unexpl, *expl = kd_node;
    double kv;//分割枢轴的值
    int ki;//分割枢轴的维数索引

    //一直搜索到叶子节点，搜索过程中将未搜索的节点根据优先级放入队列
    while (expl  &&  !expl->leaf)
    {
        ki = expl->ki;
        kv = expl->kv;

        //枢轴的维数索引大于特征点的维数，出错
        if (ki >= feat->d)
        {
            fprintf(stderr, "Warning: comparing imcompatible descriptors, %s" " line %d\n", __FILE__, __LINE__);
            return NULL;
        }
        //目标特征点ki维的数据小于等于kv，进入左子树搜索
        if (feat->descr[ki] <= kv)
        {
            unexpl = expl->kd_right;//候选搜索节点是expl的右子树的根
            expl = expl->kd_left;//当前搜索节点是expl的左子树的根
        }
        else//目标特征点ki维的数据大于kv，进入右子树搜索
        {
            unexpl = expl->kd_left;//候选搜索节点是expl的左子树
            expl = expl->kd_right;//当前搜索节点是expl的右子树
        }

        //将候选节点unexpl根据目标特征点ki维与其父节点的距离插入到优先队列中，距离越小，优先级越大
        if (minpq_insert(min_pq, unexpl, ABS(kv - feat->descr[ki])))
        {
            fprintf(stderr, "Warning: unable to insert into PQ, %s, line %d\n", __FILE__, __LINE__);
            return NULL;
        }
    }

    return expl;//返回叶子节点的指针
}

/*插入一个特征点到最近邻数组，使数组中的点按到目标点的距离升序排列
参数：
feat：要插入的特征点，其feature_data域应是指向bbf_data结构的指针，其中的d值时feat和目标点的距离的平方
nbrs：最近邻数组
n：已在最近邻数组中的元素个数
k：最近邻数组元素个数的最大值
返回值：若feat成功插入，返回1，否则返回0
*/
/*
Inserts a feature into the nearest-neighbor array so that the array remains
in order of increasing descriptor distance from the search feature.

@param feat feature to be inderted into the array; it's feature_data field
should be a pointer to a bbf_data with d equal to the squared descriptor
distance between feat and the search feature
@param nbrs array of nearest neighbors neighbors
@param n number of elements already in nbrs and
@param k maximum number of elements in nbrs

@return If feat was successfully inserted into nbrs, returns 1; otherwise
returns 0.
*/
static int insert_into_nbr_array(struct feature* feat, struct feature** nbrs, int n, int k)
{
    struct bbf_data* fdata, *ndata;//fdata是要插入的点的bbf结构，ndata是最近邻数组中的点的bbf结构
    double dn, df; //dn是最近邻数组中特征点的bbf结构中的距离值，df是要插入的特征点的bbf结构中的距离值
    int i, ret = ;

    //原最近邻数组为空
    if (n == )
    {
        nbrs[] = feat;
        return ;//插入成功，返回1
    }

    /* check at end of array */
    fdata = (struct bbf_data*)feat->feature_data;//要插入点的bbf结构
    df = fdata->d;//要插入的特征点的bbf结构中的距离值
    ndata = (struct bbf_data*)nbrs[n - ]->feature_data;//最近邻数组中的点的bbf结构
    dn = ndata->d;//最近邻数组中最后一个特征点的bbf结构中的距离值

    //df>=dn，说明要插入在最近邻数组的末尾
    if (df >= dn)
    {
        //最近邻数组中元素个数已达到最大值，无法插入
        if (n == k)
        {
            feat->feature_data = fdata->old_data;//不明白这里是干什么？
            free(fdata);
            return ;//插入失败，返回0
        }
        nbrs[n] = feat;//插入到末尾
        return ;//插入成功，返回1
    }

    //运行到此处说明插入位置不在数组末尾
    /* find the right place in the array */
    if (n < k)//最近邻数组中元素个数小于最大值，可插入
    {
        nbrs[n] = nbrs[n - ];//原数组最后一个点后移
        ret = ;//插入结果设为1
    }
    else//最近邻数组中元素个数大于或等于最大值，无法插入，插入结果ret还是0
    {//其实也不是无法插入，而是最近邻数组中元素个数不变，但值会更新
        nbrs[n - ]->feature_data = ndata->old_data;
        free(ndata);
    }
    i = n - ;
    //在最近邻数组中查找要插入的位置
    while (i >= )
    {
        ndata = (struct bbf_data*)nbrs[i]->feature_data;
        dn = ndata->d;
        if (dn <= df)//找到插入点
            break;
        nbrs[i + ] = nbrs[i];//一次后移
        i--;
    }
    i++;
    nbrs[i] = feat;//插入

    return ret;//返回结果
}

/*判断给定点是否在某矩形中
*/
/*
Determines whether a given point lies within a specified rectangular region
@param pt point
@param rect rectangular region
@return Returns 1 if pt is inside rect or 0 otherwise
*/
static int within_rect(CvPoint2D64f pt, CvRect rect)
{
    if (pt.x < rect.x || pt.y < rect.y)
        return ;
    if (pt.x > rect.x + rect.width || pt.y > rect.y + rect.height)
        return ;
    return ;
}

minpq.c

/*
Functions and structures for implementing a minimizing priority queue.

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>

@version 1.1.2-20100521
*/

/*
此文件中包括最小优先队列的实现
*/

#include "minpq.h"
#include "utils.h"

#include <limits.h>

//本地函数
/************************* Local Function Prototypes *************************/

static void restore_minpq_order(struct pq_node*, int, int);
static void decrease_pq_node_key(struct pq_node*, int, int);

//内联函数
/************************** Local Inline Functions ***************************/

//返回i节点的父节点的下标
/* returns the array index of element i's parent */
static __inline int parent(int i)
{
    return (i - ) / ;
}

//返回i节点的右孩子的下标
/* returns the array index of element i's right child */
static __inline int right(int i)
{
    return  * i + ;
}

//返回i节点的左孩子的下标
/* returns the array index of element i's left child */
static __inline int left(int i)
{
    return  * i + ;
}

/********************** Functions prototyped in minpq.h **********************/

/*初始化最小优先级队列，分配默认大小的空间
*/
/*
Creates a new minimizing priority queue.
*/
struct min_pq* minpq_init()
{
    struct min_pq* min_pq;

    min_pq = malloc(sizeof(struct min_pq));
    //默认分配MINPQ_INIT_NALLOCD个空间
    min_pq->pq_array = calloc(MINPQ_INIT_NALLOCD, sizeof(struct pq_node));
    min_pq->nallocd = MINPQ_INIT_NALLOCD;
    min_pq->n = ;

    return min_pq;
}

/**
Inserts an element into a minimizing priority queue.

@param min_pq a minimizing priority queue
@param data the data to be inserted
@param key the key to be associated with \a data

@return Returns 0 on success or 1 on failure.
*/
int minpq_insert(struct min_pq* min_pq, void* data, int key)
{
    int n = min_pq->n;

    /* double array allocation if necessary */
    if (min_pq->nallocd == n)
    {
        min_pq->nallocd = array_double(&min_pq->pq_array, min_pq->nallocd,
            sizeof(struct pq_node));
        if (!min_pq->nallocd)
        {
            fprintf(stderr, "Warning: unable to allocate memory, %s, line %d\n",
                __FILE__, __LINE__);
            return ;
        }
    }

    min_pq->pq_array[n].data = data;
    min_pq->pq_array[n].key = INT_MAX;
    decrease_pq_node_key(min_pq->pq_array, min_pq->n, key);
    min_pq->n++;

    return ;
}

/*
Returns the element of a minimizing priority queue with the smallest key
without removing it from the queue.

@param min_pq a minimizing priority queue

@return Returns the element of \a min_pq with the smallest key or NULL
if \a min_pq is empty
*/
void* minpq_get_min(struct min_pq* min_pq)
{
    if (min_pq->n < )
    {
        fprintf(stderr, "Warning: PQ empty, %s line %d\n", __FILE__, __LINE__);
        return NULL;
    }
    return min_pq->pq_array[].data;
}

/*
Removes and returns the element of a minimizing priority queue with the
smallest key.

@param min_pq a minimizing priority queue

@return Returns the element of \a min_pq with the smallest key of NULL
if \a min_pq is empty
*/
void* minpq_extract_min(struct min_pq* min_pq)
{
    void* data;

    if (min_pq->n < )
    {
        fprintf(stderr, "Warning: PQ empty, %s line %d\n", __FILE__, __LINE__);
        return NULL;
    }
    data = min_pq->pq_array[].data;
    min_pq->n--;
    min_pq->pq_array[] = min_pq->pq_array[min_pq->n];
    restore_minpq_order(min_pq->pq_array, , min_pq->n);

    return data;
}

/*
De-allocates the memory held by a minimizing priorioty queue

@param min_pq pointer to a minimizing priority queue
*/
void minpq_release(struct min_pq** min_pq)
{
    if (!min_pq)
    {
        fprintf(stderr, "Warning: NULL pointer error, %s line %d\n", __FILE__,
            __LINE__);
        return;
    }
    if (*min_pq && (*min_pq)->pq_array)
    {
        free((*min_pq)->pq_array);
        free(*min_pq);
        *min_pq = NULL;
    }
}

/************************ Functions prototyped here **************************/

/*
Decrease a minimizing pq element's key, rearranging the pq if necessary

@param pq_array minimizing priority queue array
@param i index of the element whose key is to be decreased
@param key new value of element <EM>i</EM>'s key; if greater than current
key, no action is taken
*/
static void decrease_pq_node_key(struct pq_node* pq_array, int i, int key)
{
    struct pq_node tmp;

    if (key > pq_array[i].key)
        return;

    pq_array[i].key = key;
    while (i >  && pq_array[i].key < pq_array[parent(i)].key)
    {
        tmp = pq_array[parent(i)];
        pq_array[parent(i)] = pq_array[i];
        pq_array[i] = tmp;
        i = parent(i);
    }
}

/*
Recursively restores correct priority queue order to a minimizing pq array

@param pq_array a minimizing priority queue array
@param i index at which to start reordering
@param n number of elements in \a pq_array
*/
static void restore_minpq_order(struct pq_node* pq_array, int i, int n)
{
    struct pq_node tmp;
    int l, r, min = i;

    l = left(i);
    r = right(i);
    if (l < n)
    if (pq_array[l].key < pq_array[i].key)
        min = l;
    if (r < n)
    if (pq_array[r].key < pq_array[min].key)
        min = r;

    if (min != i)
    {
        tmp = pq_array[min];
        pq_array[min] = pq_array[i];
        pq_array[i] = tmp;
        restore_minpq_order(pq_array, min, n);
    }
}

xform.c

/*
This file contains definitions for functions to compute transforms from
image feature correspondences

Copyright (C) 2006-2010  Rob Hess <hess@eecs.oregonstate.edu>

@version 1.1.2-20100521
*/

#include "xform.h"
#include "imgfeatures.h"
#include "utils.h"

#include <cxcore.h>

#include <stdlib.h>
#include <time.h>

/************************* Local Function Prototypes *************************/

static __inline struct feature* get_match(struct feature*, int);
static int get_matched_features(struct feature*, int, int, struct feature***);
static int calc_min_inliers(int, int, double, double);
static __inline double log_factorial(int);
static struct feature** draw_ransac_sample(struct feature**, int, int);
static void extract_corresp_pts(struct feature**, int, int, CvPoint2D64f**,
    CvPoint2D64f**);
static int find_consensus(struct feature**, int, int, CvMat*, ransac_err_fn,
    double, struct feature***);
static __inline void release_mem(CvPoint2D64f*, CvPoint2D64f*,
    static struct feature**);

/********************** Functions prototyped in xform.h **********************/

/*
Calculates a best-fit image transform from image feature correspondences
using RANSAC.

For more information refer to:

Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
model fitting with applications to image analysis and automated cartography.
<EM>Communications of the ACM, 24</EM>, 6 (1981), pp. 381--395.

@param features an array of features; only features with a non-NULL match
of type mtype are used in homography computation
@param n number of features in feat
@param mtype determines which of each feature's match fields to use
for model computation; should be one of FEATURE_FWD_MATCH,
FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
correspondences are assumed to be between a feature's img_pt field
and its match's mdl_pt field, otherwise correspondences are assumed to
be between the the feature's img_pt field and its match's img_pt field
@param xform_fn pointer to the function used to compute the desired
transformation from feature correspondences
@param m minimum number of correspondences necessary to instantiate the
model computed by xform_fn
@param p_badxform desired probability that the final transformation
returned by RANSAC is corrupted by outliers (i.e. the probability that
no samples of all inliers were drawn)
@param err_fn pointer to the function used to compute a measure of error
between putative correspondences and a computed model
@param err_tol correspondences within this distance of a computed model are
considered as inliers
@param inliers if not NULL, output as an array of pointers to the final
set of inliers
@param n_in if not NULL and \a inliers is not NULL, output as the final
number of inliers

@return Returns a transformation matrix computed using RANSAC or NULL
on error or if an acceptable transform could not be computed.
*/
CvMat* ransac_xform(struct feature* features, int n, int mtype,
    ransac_xform_fn xform_fn, int m, double p_badxform,
    ransac_err_fn err_fn, double err_tol,
struct feature*** inliers, int* n_in)
{
    struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;
    struct ransac_data* rdata;
    CvPoint2D64f* pts, *mpts;
    CvMat* M = NULL;
    double p, in_frac = RANSAC_INLIER_FRAC_EST;
    int i, nm, in, in_min, in_max = , k = ;

    nm = get_matched_features(features, n, mtype, &matched);
    if (nm < m)
    {
        fprintf(stderr, "Warning: not enough matches to compute xform, %s" \
            " line %d\n", __FILE__, __LINE__);
        goto end;
    }

    /* initialize random number generator */
    srand(time(NULL));

    in_min = calc_min_inliers(nm, m, RANSAC_PROB_BAD_SUPP, p_badxform);
    p = pow(1.0 - pow(in_frac, m), k);
    i = ;
    while (p > p_badxform)
    {
        sample = draw_ransac_sample(matched, nm, m);
        extract_corresp_pts(sample, m, mtype, &pts, &mpts);
        M = xform_fn(pts, mpts, m);
        if (!M)
            goto iteration_end;
        in = find_consensus(matched, nm, mtype, M, err_fn, err_tol, &consensus);
        if (in > in_max)
        {
            if (consensus_max)
                free(consensus_max);
            consensus_max = consensus;
            in_max = in;
            in_frac = (double)in_max / nm;
        }
        else
            free(consensus);
        cvReleaseMat(&M);

    iteration_end:
        release_mem(pts, mpts, sample);
        p = pow(1.0 - pow(in_frac, m), ++k);
    }

    /* calculate final transform based on best consensus set */
    if (in_max >= in_min)
    {
        extract_corresp_pts(consensus_max, in_max, mtype, &pts, &mpts);
        M = xform_fn(pts, mpts, in_max);
        in = find_consensus(matched, nm, mtype, M, err_fn, err_tol, &consensus);
        cvReleaseMat(&M);
        release_mem(pts, mpts, consensus_max);
        extract_corresp_pts(consensus, in, mtype, &pts, &mpts);
        M = xform_fn(pts, mpts, in);
        if (inliers)
        {
            *inliers = consensus;
            consensus = NULL;
        }
        if (n_in)
            *n_in = in;
        release_mem(pts, mpts, consensus);
    }
    else if (consensus_max)
    {
        if (inliers)
            *inliers = NULL;
        if (n_in)
            *n_in = ;
        free(consensus_max);
    }

end:
    for (i = ; i < nm; i++)
    {
        rdata = feat_ransac_data(matched[i]);
        matched[i]->feature_data = rdata->orig_feat_data;
        free(rdata);
    }
    free(matched);
    return M;
}

/*
Calculates a planar homography from point correspondeces using the direct
linear transform.  Intended for use as a ransac_xform_fn.

@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1,
corresponds to mpts[i]
@param n number of points in both pts and mpts; must be at least 4

@return Returns the 3x3 planar homography matrix that transforms points
in pts to their corresponding points in mpts or NULL if fewer than 4
correspondences were provided
*/
CvMat* dlt_homog(CvPoint2D64f* pts, CvPoint2D64f* mpts, int n)
{
    CvMat* H, *A, *VT, *D, h, v9;
    double _h[];
    int i;

    if (n < )
        return NULL;

    /* set up matrices so we can unstack homography into h; Ah = 0 */
    A = cvCreateMat( * n, , CV_64FC1);
    cvZero(A);
    for (i = ; i < n; i++)
    {
        cvmSet(A,  * i, , -pts[i].x);
        cvmSet(A,  * i, , -pts[i].y);
        cvmSet(A,  * i, , -1.0);
        cvmSet(A,  * i, , mpts[i].y * pts[i].x);
        cvmSet(A,  * i, , mpts[i].y * pts[i].y);
        cvmSet(A,  * i, , mpts[i].y);
        cvmSet(A,  * i + , , pts[i].x);
        cvmSet(A,  * i + , , pts[i].y);
        cvmSet(A,  * i + , , 1.0);
        cvmSet(A,  * i + , , -mpts[i].x * pts[i].x);
        cvmSet(A,  * i + , , -mpts[i].x * pts[i].y);
        cvmSet(A,  * i + , , -mpts[i].x);
    }
    D = cvCreateMat(, , CV_64FC1);
    VT = cvCreateMat(, , CV_64FC1);
    cvSVD(A, D, NULL, VT, CV_SVD_MODIFY_A + CV_SVD_V_T);
    v9 = cvMat(, , CV_64FC1, NULL);
    cvGetRow(VT, &v9, );
    h = cvMat(, , CV_64FC1, _h);
    cvCopy(&v9, &h, NULL);
    h = cvMat(, , CV_64FC1, _h);
    H = cvCreateMat(, , CV_64FC1);
    cvConvert(&h, H);

    cvReleaseMat(&A);
    cvReleaseMat(&D);
    cvReleaseMat(&VT);
    return H;
}

/*
Calculates a least-squares planar homography from point correspondeces.

@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1, corresponds
to mpts[i]
@param n number of points in both pts and mpts; must be at least 4

@return Returns the 3 x 3 least-squares planar homography matrix that
transforms points in pts to their corresponding points in mpts or NULL if
fewer than 4 correspondences were provided
*/
CvMat* lsq_homog(CvPoint2D64f* pts, CvPoint2D64f* mpts, int n)
{
    CvMat* H, *A, *B, X;
    double x[];
    int i;

    if (n < )
    {
        fprintf(stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
            __FILE__, __LINE__);
        return NULL;
    }

    /* set up matrices so we can unstack homography into X; AX = B */
    A = cvCreateMat( * n, , CV_64FC1);
    B = cvCreateMat( * n, , CV_64FC1);
    X = cvMat(, , CV_64FC1, x);
    H = cvCreateMat(, , CV_64FC1);
    cvZero(A);
    for (i = ; i < n; i++)
    {
        cvmSet(A, i, , pts[i].x);
        cvmSet(A, i + n, , pts[i].x);
        cvmSet(A, i, , pts[i].y);
        cvmSet(A, i + n, , pts[i].y);
        cvmSet(A, i, , 1.0);
        cvmSet(A, i + n, , 1.0);
        cvmSet(A, i, , -pts[i].x * mpts[i].x);
        cvmSet(A, i, , -pts[i].y * mpts[i].x);
        cvmSet(A, i + n, , -pts[i].x * mpts[i].y);
        cvmSet(A, i + n, , -pts[i].y * mpts[i].y);
        cvmSet(B, i, , mpts[i].x);
        cvmSet(B, i + n, , mpts[i].y);
    }
    cvSolve(A, B, &X, CV_SVD);
    x[] = 1.0;
    X = cvMat(, , CV_64FC1, x);
    cvConvert(&X, H);

    cvReleaseMat(&A);
    cvReleaseMat(&B);
    return H;
}

/*
Calculates the transfer error between a point and its correspondence for
a given homography, i.e. for a point x, it's correspondence x', and
homography H, computes d(x', Hx)^2.

@param pt a point
@param mpt pt's correspondence
@param H a homography matrix

@return Returns the transfer error between pt and mpt given H
*/
double homog_xfer_err(CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H)
{
    CvPoint2D64f xpt = persp_xform_pt(pt, H);

    return sqrt(dist_sq_2D(xpt, mpt));
}

/*
Performs a perspective transformation on a single point.  That is, for a
point (x, y) and a 3 x 3 matrix T this function returns the point
(u, v), where

[x' y' w']^T = T * [x y 1]^T,

and

(u, v) = (x'/w', y'/w').

Note that affine transforms are a subset of perspective transforms.

@param pt a 2D point
@param T a perspective transformation matrix

@return Returns the point (u, v) as above.
*/
CvPoint2D64f persp_xform_pt(CvPoint2D64f pt, CvMat* T)
{
    CvMat XY, UV;
    double xy[] = { pt.x, pt.y, 1.0 }, uv[] = {  };
    CvPoint2D64f rslt;

    cvInitMatHeader(&XY, , , CV_64FC1, xy, CV_AUTOSTEP);
    cvInitMatHeader(&UV, , , CV_64FC1, uv, CV_AUTOSTEP);
    cvMatMul(T, &XY, &UV);
    rslt = cvPoint2D64f(uv[] / uv[], uv[] / uv[]);

    return rslt;
}

/************************ Local funciton definitions *************************/

/*
Returns a feature's match according to a specified match type

@param feat feature
@param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
FEATURE_MDL_MATCH

@return Returns feat's match corresponding to mtype or NULL for bad mtype
*/
static __inline struct feature* get_match(struct feature* feat, int mtype)
{
    if (mtype == FEATURE_MDL_MATCH)
        return feat->mdl_match;
    if (mtype == FEATURE_BCK_MATCH)
        return feat->bck_match;
    if (mtype == FEATURE_FWD_MATCH)
        return feat->fwd_match;
    return NULL;
}

/*
Finds all features with a match of a specified type and stores pointers
to them in an array.  Additionally initializes each matched feature's
feature_data field with a ransac_data structure.

@param features array of features
@param n number of features in features
@param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
@param matched output as an array of pointers to features with a match of
the specified type

@return Returns the number of features output in matched.
*/
static int get_matched_features(struct feature* features, int n, int mtype,
struct feature*** matched)
{
    struct feature** _matched;
    struct ransac_data* rdata;
    int i, m = ;

    _matched = calloc(n, sizeof(struct feature*));
    for (i = ; i < n; i++)
    if (get_match(features + i, mtype))
    {
        rdata = malloc(sizeof(struct ransac_data));
        memset(rdata, , sizeof(struct ransac_data));
        rdata->orig_feat_data = features[i].feature_data;
        _matched[m] = features + i;
        _matched[m]->feature_data = rdata;
        m++;
    }
    *matched = _matched;
    return m;
}

/*
Calculates the minimum number of inliers as a function of the number of
putative correspondences.  Based on equation (7) in

Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
In <EM>Conference on Computer Vision and Pattern Recognition (CVPR)</EM>,
(2005), pp. 220--226.

@param n number of putative correspondences
@param m min number of correspondences to compute the model in question
@param p_badsupp prob. that a bad model is supported by a correspondence
@param p_badxform desired prob. that the final transformation returned is bad

@return Returns the minimum number of inliers required to guarantee, based
on p_badsupp, that the probability that the final transformation returned
by RANSAC is less than p_badxform
*/
static int calc_min_inliers(int n, int m, double p_badsupp, double p_badxform)
{
    double pi, sum;
    int i, j;

    for (j = m + ; j <= n; j++)
    {
        sum = ;
        for (i = j; i <= n; i++)
        {
            pi = (i - m) * log(p_badsupp) + (n - i + m) * log(1.0 - p_badsupp) +
                log_factorial(n - m) - log_factorial(i - m) -
                log_factorial(n - i);
            /*
            * Last three terms above are equivalent to log( n-m choose i-m )
            */
            sum += exp(pi);
        }
        if (sum < p_badxform)
            break;
    }
    return j;
}

/*
Calculates the natural log of the factorial of a number

@param n number

@return Returns log( n! )
*/
static __inline double log_factorial(int n)
{
    double f = ;
    int i;

    for (i = ; i <= n; i++)
        f += log(i);

    return f;
}

/*
Draws a RANSAC sample from a set of features.

@param features array of pointers to features from which to sample
@param n number of features in features
@param m size of the sample

@return Returns an array of pointers to the sampled features; the sampled
field of each sampled feature's ransac_data is set to 1
*/
static struct feature** draw_ransac_sample(struct feature** features, int n, int m)
{
    struct feature** sample, *feat;
    struct ransac_data* rdata;
    int i, x;

    for (i = ; i < n; i++)
    {
        rdata = feat_ransac_data(features[i]);
        rdata->sampled = ;
    }

    sample = calloc(m, sizeof(struct feature*));
    for (i = ; i < m; i++)
    {
        do
        {
            x = rand() % n;
            feat = features[x];
            rdata = feat_ransac_data(feat);
        } while (rdata->sampled);
        sample[i] = feat;
        rdata->sampled = ;
    }

    return sample;
}

/*
Extrancs raw point correspondence locations from a set of features

@param features array of features from which to extract points and match
points; each of these is assumed to have a match of type mtype
@param n number of features
@param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
to be between each feature's img_pt field and it's match's mdl_pt field,
otherwise, correspondences are assumed to be between img_pt and img_pt
@param pts output as an array of raw point locations from features
@param mpts output as an array of raw point locations from features' matches
*/
static void extract_corresp_pts(struct feature** features, int n, int mtype,
    CvPoint2D64f** pts, CvPoint2D64f** mpts)
{
    struct feature* match;
    CvPoint2D64f* _pts, *_mpts;
    int i;

    _pts = calloc(n, sizeof(CvPoint2D64f));
    _mpts = calloc(n, sizeof(CvPoint2D64f));

    if (mtype == FEATURE_MDL_MATCH)
    for (i = ; i < n; i++)
    {
        match = get_match(features[i], mtype);
        if (!match)
            fatal_error("feature does not have match of type %d, %s line %d",
            mtype, __FILE__, __LINE__);
        _pts[i] = features[i]->img_pt;
        _mpts[i] = match->mdl_pt;
    }

    else
    for (i = ; i < n; i++)
    {
        match = get_match(features[i], mtype);
        if (!match)
            fatal_error("feature does not have match of type %d, %s line %d",
            mtype, __FILE__, __LINE__);
        _pts[i] = features[i]->img_pt;
        _mpts[i] = match->img_pt;
    }

    *pts = _pts;
    *mpts = _mpts;
}

/*
For a given model and error function, finds a consensus from a set of
feature correspondences.

@param features set of pointers to features; every feature is assumed to
have a match of type mtype
@param n number of features in features
@param mtype determines the match field of each feature against which to
measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
to be between the feature's img_pt field and the match's mdl_pt field;
otherwise matches are assumed to be between img_pt and img_pt
@param M model for which a consensus set is being found
@param err_fn error function used to measure distance from M
@param err_tol correspondences within this distance of M are added to the
consensus set
@param consensus output as an array of pointers to features in the
consensus set

@return Returns the number of points in the consensus set
*/
static int find_consensus(struct feature** features, int n, int mtype,
    CvMat* M, ransac_err_fn err_fn, double err_tol,
struct feature*** consensus)
{
    struct feature** _consensus;
    struct feature* match;
    CvPoint2D64f pt, mpt;
    double err;
    int i, in = ;

    _consensus = calloc(n, sizeof(struct feature*));

    if (mtype == FEATURE_MDL_MATCH)
    for (i = ; i < n; i++)
    {
        match = get_match(features[i], mtype);
        if (!match)
            fatal_error("feature does not have match of type %d, %s line %d",
            mtype, __FILE__, __LINE__);
        pt = features[i]->img_pt;
        mpt = match->mdl_pt;
        err = err_fn(pt, mpt, M);
        if (err <= err_tol)
            _consensus[in++] = features[i];
    }

    else
    for (i = ; i < n; i++)
    {
        match = get_match(features[i], mtype);
        if (!match)
            fatal_error("feature does not have match of type %d, %s line %d",
            mtype, __FILE__, __LINE__);
        pt = features[i]->img_pt;
        mpt = match->img_pt;
        err = err_fn(pt, mpt, M);
        if (err <= err_tol)
            _consensus[in++] = features[i];
    }
    *consensus = _consensus;
    return in;
}

/*
Releases memory and reduces code size above

@param pts1 an array of points
@param pts2 an array of points
@param features an array of pointers to features; can be NULL
*/
static __inline void release_mem(CvPoint2D64f* pts1, CvPoint2D64f* pts2,
struct feature** features)
{
    free(pts1);
    free(pts2);
    if (features)
        free(features);
}