原算法来源于Xinhao Liu和Masayuki Tanaka发表在TIP上的三篇论文,程序根据matlab工具箱源程序转换而来的C++版本,非原创算法,仅供参考