【源代码】Image Deformation Using Moving Least Squares算法的实现

在参考https://code.google.com/p/imgwarp-opencv/的基础上，把mls图像变形算法进行了实现，但感觉效果不是很好。在此贴出源代码，希望有人其同探讨与对其改进。

                  

原图像                                                                                     变形后图像

       关于像素值的复原：mls只讲变形后新坐标的位置，未谈到像素值如何还原（因为一般为整数坐标到实数的坐标）。前向映射与逆向映射2种方法我都试过，效果一般，网格明显。有没有更好的像素值复原办法呢？

关于本代码：对于控制点8个以上变形就会出错（有一点在轮廓内），如此图。不知是什么原因。

 附代码（依赖于opencv）

/*****************************************************************************************头文件*******************************************************************/

#ifndef MLS_DEFORMATION_H
#define MLS_DEFORMATION_H
 
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
 
 
class MLSDeformation
{
public:
    virtual boolInitMLSD(const cv::Mat& oriImg,const std::vector<cv::Point2i>&src_pt,int alpha=1, int gridsize=5);
    virtualcv::Mat Deformation(const std::vector<cv::Point2i> &dst_pt)=0;
    explicitMLSDeformation();
    virtual ~MLSDeformation();
protected:
    cv::MatgetDeformationImg();
    voidmeanMat(cv::Mat& mat, int i, int j);//对mat(i,j)进行gridsize的中值（当mean为空时），不包括像素为空的坐标
    voidrepairMat(cv::Mat& mat);//变形后的mat有些位置没有像素点，使用中值滤波填补
    int gridsize_;
    int alpha_;///weight_计算相关
    intn_point_;///控制点数
    cv::Matsrc_img_;//原图像
    intsrc_width_;
    intsrc_height_;
   std::vector<cv::Point2d> pi_point_;//原控制点
   std::vector<cv::Point2d> qi_point_;//变形后的控制点
    ///参数信息
    double*weight_;//权重w*h*pt
    double*sum_weight_;//w*h
    cv::Point2d*deformation_pt_;//对应目标坐标
    cv::Point2dpstart_, qstart_;
   std::vector<cv::Point2d> pdiff_, qdiff_;
};
 
class MLSDAffine :public MLSDeformation
{
public:
    virtual boolInitMLSD(const cv::Mat& oriImg,const std::vector<cv::Point2i>&src_pt,int alpha=1, int gridsize=5);
    virtualcv::Mat Deformation(const std::vector<cv::Point2i> &dst_pt);
    explicitMLSDAffine();
    virtual~MLSDAffine();
private:
    double*a_affine_;///affine A参数
};
 
class MLSDSimilarity :public MLSDeformation
{
public:
    virtual boolInitMLSD(const cv::Mat& oriImg,const std::vector<cv::Point2i>&src_pt,int alpha=1, int gridsize=5);
    virtualcv::Mat Deformation(const std::vector<cv::Point2i> &dst_pt);
    explicitMLSDSimilarity();
    virtual~MLSDSimilarity();
private:
    double*us_;//us参数
    cv::Matx22d*a_similarity_;///similarity A参数
};
 
class MLSDRigid :public MLSDeformation
{
public:
    virtual boolInitMLSD(const cv::Mat& oriImg,const std::vector<cv::Point2i>&src_pt,int alpha=1, int gridsize=5);
    virtualcv::Mat Deformation(const std::vector<cv::Point2i> &dst_pt);
    explicitMLSDRigid();
    virtual~MLSDRigid();
private:
    double*ur_;//us参数
    cv::Point2d*pstart_rigid_;//
    cv::Matx22d*a_rigid_;///similarity A参数
};
#endif // MLS_DEFORMATION_H

 

/*****************************************************************************************源文件***************************************************************/
#include "MLSDeformation.h"
#include"../imageMLSwrap_gui/UseConsole.h"
 
#defineWeight(i,j,point) (weight_[(i)*src_height_+(j)*n_point_ + (point)])
#defineSumWeight(i,j) (sum_weight_[(i)*src_height_+(j)])
#defineDefPT(i,j) (deformation_pt_[(i)*src_height_+(j)])
#define US(i,j)(us_[(i)*src_height_+(j)])
#define UR(i,j)(ur_[(i)*src_height_+(j)])
 
#defineAffine(i,j,point) (a_affine_[(i)*src_height_+(j)*n_point_ + (point)])
#defineSimilarity(i,j,point) (a_similarity_[(i)*src_height_+(j)*n_point_ + (point)])
#defineRigid(i,j,point) (a_rigid_[(i)*src_height_+(j)*n_point_ + (point)])
#defineRigidPStart(i,j) (pstart_rigid_[(i)*src_height_+(j)])
 
inline doublebilinear_interp(double x, double y,
                              double v11,double v12, double v21, double v22) ///双线性插值
{
    return (v11*(1-y) + v12*y) * (1-x) +(v21*(1-y) + v22*y) * x;
}
 
MLSDeformation::MLSDeformation()
   :weight_(NULL),sum_weight_(NULL),deformation_pt_(NULL)
{
 
}
 
MLSDeformation::~MLSDeformation()
{
    if(weight_) {
        delete []weight_;
        weight_=NULL;
    }
    if(sum_weight_) {
        delete []sum_weight_;
        sum_weight_=NULL;
    }
    if(deformation_pt_) {
        delete []deformation_pt_;
        deformation_pt_=NULL;
    }
}
 
boolMLSDeformation::InitMLSD(const cv::Mat& oriImg, conststd::vector<cv::Point2i>& src_pt, int alpha, int gridsize)
{
    n_point_ = src_pt.size();
    if(n_point_<2) {
        return false;    ///点太少
    }
    src_img_=oriImg.clone();
    src_width_=oriImg.cols;
    src_height_=oriImg.rows;
    alpha_=alpha;
    gridsize_=gridsize;
    if(weight_) {
        delete []weight_;
    }
    weight_=new double[src_width_ * src_height_* n_point_];
    if(sum_weight_) {
        delete []sum_weight_;
    }
    sum_weight_=new double[src_width_ *src_height_];
    if(deformation_pt_) {
        delete []deformation_pt_;
    }
    deformation_pt_=new cv::Point2d[src_width_* src_height_];
    pi_point_.resize(n_point_);
    for (int i = 0; i<n_point_; ++i) {
        pi_point_[i]=src_pt[i];
    }
    return true;
}
 
voidMLSDeformation::meanMat(cv::Mat& mat, int i, int j)
{
    cv::Rect matrc(0,0,mat.cols,mat.rows);
    cv::Rect rc(0,0,gridsize_,gridsize_);
    rc+=cv::Point2i(i*0.5,j*0.5);
    rc&=matrc;
    cv::Mat mean_mat(mat,rc);
    cv::Mat mask;
    cv::cvtColor(mean_mat,mask,CV_BGR2GRAY);
    cv::Scalar piex=cv::mean(mean_mat,mask);
    mat.at<cv::Vec3b>(j,i)[0]=piex[0];
    mat.at<cv::Vec3b>(j,i)[1]=piex[1];
    mat.at<cv::Vec3b>(j,i)[2]=piex[2];
}
voidMLSDeformation::repairMat(cv::Mat& mat)
{
    for(int i=0; i<src_width_; ++i) {
        for(int j=0; j<src_height_; ++j) {
            cv::Vec3bpiex=mat.at<cv::Vec3b>(j,i);
            if(piex[0]==0 && piex[1]==0&& piex[2]==0) {
                meanMat(mat,i,j);
            }
        }
    }
}
cv::MatMLSDeformation::getDeformationImg()
{
    cv::MatdstImg=cv::Mat::zeros(src_height_,src_width_,src_img_.type());
    int nleft,ntop,nbottom,nright;//grid四个点的坐标信息
    cv::Point2d lt,rt,lb,rb;//相对应于DefPT的坐标
    cv::Vec3d piex;//像素值
    for(int i=0; i<src_width_; i+=gridsize_){
        for(int j=0; j<src_height_;j+=gridsize_) {
            ////gridsize操作
           nleft=i,ntop=j,nright=i+gridsize_,nbottom=j+gridsize_;
            if(nright>src_width_-1) {
                nright=src_width_-1;
            }
            if(nbottom>src_height_-1) {
                nbottom=src_height_-1;
            }
            lt=DefPT(nleft,ntop);
            lb=DefPT(nleft,nbottom);
            rt=DefPT(nright,ntop);
            rb=DefPT(nright,nbottom);
            for(int di=nleft; di<nright;++di) {
                for(int dj=ntop; dj<nbottom;++dj) {
                    doubledeltax=bilinear_interp(1.*(di-nleft)/(nright-nleft),1.*(dj-ntop)/(nbottom-ntop),lt.x,rt.x,lb.x,rb.x);
                    doubledeltay=bilinear_interp(1.*(di-nleft)/(nright-nleft),1.*(dj-ntop)/(nbottom-ntop),lt.y,rt.y,lb.y,rb.y);
                    cv::Point2i pt[2];
                    deltax=di-deltax;
                    deltay=dj-deltay;
 
                    pt[0].x=floor(deltax);
                    pt[0].y=floor(deltay);
                    pt[1].x=ceil(deltax);
                    pt[1].y=ceil(deltay);
                    for(int i=0; i<2; ++i) {
                        if(pt[i].x<0) {
                            pt[i].x=0;
                        } elseif(pt[i].x>src_width_-1) {
                           pt[i].x=src_width_-1;
                        }
                        if(pt[i].y<0) {
                            pt[i].y=0;
                        } elseif(pt[i].y>src_height_-1) {
                           pt[i].y=src_height_-1;
                        }
                    }
//
                    for (int ll=0; ll<3;ll++)
                       dstImg.at<cv::Vec3b>(dj, di)[ll]=bilinear_interp(pt[1].x-deltax,pt[1].y-deltay,
                                                         src_img_.at<cv::Vec3b>(pt[0].y, pt[0].x)[ll],
                                                         src_img_.at<cv::Vec3b>(pt[1].y, pt[0].x)[ll],
                                                         src_img_.at<cv::Vec3b>(pt[0].y, pt[1].x)[ll],
                                                         src_img_.at<cv::Vec3b>(pt[1].y, pt[1].x)[ll]);
 
//
//                    for(int i=0; i<2; ++i) {
//                        for(int j=0; j<2;++j) {
//                           piex=dstImg.at<cv::Vec3b>(pt[i].y,pt[j].x);
//                            if(piex[0]==0&& piex[1]==0 && piex[2]==0) {
//                               dstImg.at<cv::Vec3b>(pt[i].y,pt[j].x)=src_img_.at<cv::Vec3b>(dj,di);
//                            } else {
//                               piex+=src_img_.at<cv::Vec3b>(dj,di);
//                               dstImg.at<cv::Vec3b>(pt[i].y,pt[j].x)=piex*0.5;
//                            }
//                        }
//                    }
                }
            }
        }
    }
    return dstImg;
}
 
//////////////////////////////////////////////////////////////////////MLSDAffineclass/////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MLSDAffine::MLSDAffine()
    :a_affine_(NULL)
{
 
}
 
MLSDAffine::~MLSDAffine()
{
    if(a_affine_) {
        delete []a_affine_;
        a_affine_=NULL;
    }
}
 
boolMLSDAffine::InitMLSD(const cv::Mat& oriImg, conststd::vector<cv::Point2i>& src_pt, int alpha, int gridsize)
{
   if(!MLSDeformation::InitMLSD(oriImg,src_pt,alpha,gridsize)) {
        return false;
    }
    if(a_affine_) {
        delete []a_affine_;
    }
    a_affine_=new double[src_width_ *src_height_ * n_point_];
 
    pdiff_.resize(n_point_);
    cv::Point2d tmp_pt2d;
    for(int i=0; i<src_width_; i+=gridsize_){
        for(int j=0; j<src_height_;j+=gridsize_) {
            ///由pi计算pstart,pdiff
            cv::Point2d piex_pt(i,j);
            SumWeight(i,j)=0.;
            pstart_=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(src_pt[index].x ==i&& src_pt[index].y == j) {
                    Weight(i,j,index)=0.;
                    continue;
                }
               tmp_pt2d=pi_point_[index]-piex_pt;
                if(alpha_ ==1) {
                    Weight(i,j,index)=1/tmp_pt2d.ddot(tmp_pt2d);
                } else {
                   Weight(i,j,index)=pow(tmp_pt2d.ddot(tmp_pt2d),-alpha_);
                }
               SumWeight(i,j)+=Weight(i,j,index);
                pstart_+=Weight(i,j,index)*pi_point_[index];
            }
            pstart_*=(1/SumWeight(i,j));
            for(int index=0; index<n_point_;++index) {
               pdiff_[index]=pi_point_[index]-pstart_;
            }
 
///aj=(piex-pstart)*sum(pdifftwipdiff).inv()*wj*pjt
            cv::Matsummat=cv::Mat::zeros(2,2,CV_64F);
            for(int index=0; index<n_point_;++index) {//
                cv::MatpointMat(pdiff_[index]);
               summat+=Weight(i,j,index)*pointMat*pointMat.t();
            }
            summat=summat.inv();
            tmp_pt2d=piex_pt-pstart_;//v-p*
            for(int index=0; index<n_point_;++index) {//sum()
                cv::Mat pointMat(tmp_pt2d);
                pointMat=pointMat.t() * summat;
                pointMat=Weight(i,j,index)*pointMat*cv::Mat(pdiff_[index]);
               Affine(i,j,index)=pointMat.at<double>(0,0);//只有一个元素
            }
        }
    }
    return true;
}
 
cv::MatMLSDAffine::Deformation(const std::vector<cv::Point2i>& dst_pt)
{
    if(n_point_ != dst_pt.size()) {
        return cv::Mat();
    }
    qi_point_.resize(n_point_);
    qdiff_.resize(n_point_);
    for (int i = 0; i<n_point_; ++i) {
        qi_point_[i]=dst_pt[i];
    }
    for(int i=0;; i+=gridsize_) {
        if (i>=src_width_ &&i<src_width_+gridsize_ - 1) { //计算最后一行一列
            i=src_width_-1;
        } else if (i>=src_width_) {
            break;
        }
        for (int j = 0; ; j+=gridsize_) {
            if (j>=src_height_ &&j<src_height_+gridsize_ - 1) { //计算最后一行一列
                j = src_height_ - 1;
            } else if (j>=src_height_) {
                break;
            }
            ///由qi计算qstart,qdiff
            bool isequalpi=false;
            cv::Point2d piex_pt(i,j);
            qstart_=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(pi_point_[index].x == i&& pi_point_[index].y ==j ) {
                    isequalpi=true;
                    break;
                }
                qstart_+=Weight(i,j,index)*qi_point_[index];
            }
            if(isequalpi) {
                DefPT(i,j)=cv::Point2d(0,0);
            } else {
                qstart_*=(1/SumWeight(i,j));
                for(int index=0;index<n_point_; ++index) {
                    qdiff_[index]=qi_point_[index]-qstart_;
                }
                ///fj=sum(aj*qdiffj)+qstart
                cv::Matsummat=cv::Mat::zeros(2,1,CV_64F);
                for(int index=0;index<n_point_; ++index) {
                    cv::Mat pointMat(qdiff_[index]);
                   summat+=Affine(i,j,index)*pointMat;
                }
                summat+=cv::Mat(qstart_);
               DefPT(i,j).x=summat.at<double>(0,0)-i;
               DefPT(i,j).y=summat.at<double>(1,0)-j;
            }
        }
    }
    return getDeformationImg();
}
 
//////////////////////////////////////////////////MLSDSimilarityclass/////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MLSDSimilarity::MLSDSimilarity()
    : a_similarity_(NULL),us_(NULL)
{
 
}
 
MLSDSimilarity::~MLSDSimilarity()
{
    if(a_similarity_) {
        delete []a_similarity_;
        a_similarity_=NULL;
    }
    if(us_) {
        delete []us_;
        us_=NULL;
    }
}
 
boolMLSDSimilarity::InitMLSD(const cv::Mat& oriImg, conststd::vector<cv::Point2i>& src_pt, int alpha, int gridsize)
{
   if(!MLSDeformation::InitMLSD(oriImg,src_pt,alpha,gridsize)) {
        return false;
    }
    if(a_similarity_) {
        delete []a_similarity_;
    }
    a_similarity_=new cv::Matx22d[src_width_ *src_height_ * n_point_];
    if(us_) {
        delete []us_;
    }
    us_=new double[src_width_ * src_height_];
 
    pdiff_.resize(n_point_);
    cv::Point2d tmp_pt2d;
    for(int i=0; i<src_width_; i+=gridsize_){
        for(int j=0; j<src_height_;j+=gridsize_) {
            ///由pi计算pstart,pdiff
            cv::Point2d piex_pt(i,j);
            SumWeight(i,j)=0.;
            pstart_=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(src_pt[index].x ==i&& src_pt[index].y == j) {
                    Weight(i,j,index)=0.;
                    continue;
                }
                tmp_pt2d=pi_point_[index]-piex_pt;
                if(alpha_ ==1) {
                   Weight(i,j,index)=1/tmp_pt2d.ddot(tmp_pt2d);
                } else {
                   Weight(i,j,index)=pow(tmp_pt2d.ddot(tmp_pt2d),-alpha_);
                }
                SumWeight(i,j)+=Weight(i,j,index);
               pstart_+=Weight(i,j,index)*pi_point_[index];
            }
            pstart_*=(1/SumWeight(i,j));
            US(i,j)=0.;
            for(int index=0; index<n_point_;++index) {
               pdiff_[index]=pi_point_[index]-pstart_;
               US(i,j)+=Weight(i,j,index)*pdiff_[index].ddot(pdiff_[index]);
            }
 
///ai=wi * (pi-pi`)T(v-pstart -(v-pstart)`)
            cv::Matx22dleftmat=cv::Matx22d::zeros();
            cv::Matx22d rightmat=cv::Matx22d::zeros();
            cv::Point2dpdiff_plus,vpstart,vpstart_plus;
            for(int index=0; index<n_point_;++index) {
                pdiff_plus.x=-pdiff_[index].y;
                pdiff_plus.y=pdiff_[index].x;
                vpstart=piex_pt-pstart_;
                vpstart_plus.x=vpstart.y;
                vpstart_plus.y=-vpstart.x;
                leftmat(0,0)=pdiff_[index].x;
                leftmat(1,0)=pdiff_[index].y;
                leftmat(0,1)=pdiff_plus.x;
                leftmat(1,1)=pdiff_plus.y;
                rightmat(0,0)=vpstart.x;
                rightmat(1,0)=vpstart.y;
                rightmat(0,1)=vpstart_plus.x;
                rightmat(1,1)=vpstart_plus.y;
                Similarity(i,j,index)=Weight(i,j,index)*leftmat*rightmat.t();
            }
        }
    }
    return true;
}
 
cv::MatMLSDSimilarity::Deformation(const std::vector<cv::Point2i>& dst_pt)
{
    if(n_point_ != dst_pt.size()) {
        return cv::Mat();
    }
    qi_point_.resize(n_point_);
    qdiff_.resize(n_point_);
    for (int i = 0; i<n_point_; ++i) {
        qi_point_[i]=dst_pt[i];
    }
    for(int i=0;; i+=gridsize_) {
        if (i>=src_width_ &&i<src_width_+gridsize_ - 1) { //计算最后一行一列
            i=src_width_-1;
        } else if (i>=src_width_) {
            break;
        }
        for (int j = 0; ; j+=gridsize_) {
            if (j>=src_height_ &&j<src_height_+gridsize_ - 1) { //计算最后一行一列
                j = src_height_ - 1;
            } else if (j>=src_height_) {
                break;
            }
            ///由qi计算qstart,qdiff
            bool isequalpi=false;
            cv::Point2d piex_pt(i,j);
            qstart_=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(pi_point_[index].x == i&& pi_point_[index].y ==j ) {
                    isequalpi=true;
                    break;
                }
               qstart_+=Weight(i,j,index)*qi_point_[index];
            }
            if(isequalpi) {
                DefPT(i,j)=cv::Point2d(0,0);
            } else {
                qstart_*=(1/SumWeight(i,j));
                for(int index=0;index<n_point_; ++index) {
                   qdiff_[index]=qi_point_[index]-qstart_;
                }
                ///fj=sum(qi*Ai/us)+qstart
                cv::Matsummat=cv::Mat::zeros(1,2,CV_64F);
                for(int index=0;index<n_point_; ++index) {
                    cv::MatpointMat(qdiff_[index]);
                    summat+=pointMat.t()*cv::Mat(Similarity(i,j,index))*(1./US(i,j));
                }
                summat+=cv::Mat(qstart_).t();
               DefPT(i,j).x=summat.at<double>(0,0)-i;
               DefPT(i,j).y=summat.at<double>(0,1)-j;
            }
        }
    }
    return getDeformationImg();
}
 
//////////////////////////////////////////////////MLSDRigidclass/////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MLSDRigid::MLSDRigid()
    :a_rigid_(NULL),ur_(NULL)
    ,pstart_rigid_(NULL)
{
 
}
 
MLSDRigid::~MLSDRigid()
{
    if(a_rigid_) {
        delete []a_rigid_;
        a_rigid_=NULL;
    }
    if(ur_) {
        delete []ur_;
        ur_=NULL;
    }
    if(pstart_rigid_) {
        delete []pstart_rigid_;
        pstart_rigid_=NULL;
    }
}
 
boolMLSDRigid::InitMLSD(const cv::Mat& oriImg, const std::vector<cv::Point2i>&src_pt, int alpha, int gridsize)
{
   if(!MLSDeformation::InitMLSD(oriImg,src_pt,alpha,gridsize)) {
        return false;
    }
    if(a_rigid_) {
        delete []a_rigid_;
    }
 
    a_rigid_=new cv::Matx22d[src_width_ * src_height_* n_point_];
    if(ur_) {
        delete []ur_;
    }
    ur_=new double[src_width_ * src_height_];
    if(pstart_rigid_) {
        delete []pstart_rigid_;
    }
    pstart_rigid_=new cv::Point2d[src_width_ *src_height_];
 
    pdiff_.resize(n_point_);
    cv::Point2d tmp_pt2d;
    for(int i=0; i<src_width_; i+=gridsize_){
        for(int j=0; j<src_height_;j+=gridsize_) {
            ///由pi计算pstart,pdiff
            cv::Point2d piex_pt(i,j);
            SumWeight(i,j)=0.;
            RigidPStart(i,j)=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(src_pt[index].x ==i&& src_pt[index].y == j) {
                    Weight(i,j,index)=0.;
                    continue;
                }
               tmp_pt2d=pi_point_[index]-piex_pt;
                if(alpha_ ==1) {
                   Weight(i,j,index)=1/tmp_pt2d.ddot(tmp_pt2d);
                } else {
                   Weight(i,j,index)=pow(tmp_pt2d.ddot(tmp_pt2d),-alpha_);
                }
               SumWeight(i,j)+=Weight(i,j,index);
               pstart_+=Weight(i,j,index)*pi_point_[index];
               RigidPStart(i,j)+=Weight(i,j,index)*pi_point_[index];
            }
           RigidPStart(i,j)*=(1/SumWeight(i,j));
            for(int index=0; index<n_point_;++index) {
               pdiff_[index]=pi_point_[index]-RigidPStart(i,j);
            }
 
///ai=wi * (pi-pi`)T(v-pstart -(v-pstart)`)
            cv::Matx22dleftmat=cv::Matx22d::zeros();
            cv::Matx22drightmat=cv::Matx22d::zeros();
            cv::Point2dpdiff_plus,vpstart,vpstart_plus;
            for(int index=0; index<n_point_;++index) {
                pdiff_plus.x=-pdiff_[index].y;
                pdiff_plus.y=pdiff_[index].x;
                vpstart=piex_pt-RigidPStart(i,j);
                vpstart_plus.x=vpstart.y;
                vpstart_plus.y=-vpstart.x;
                leftmat(0,0)=pdiff_[index].x;
                leftmat(1,0)=pdiff_[index].y;
                leftmat(0,1)=pdiff_plus.x;
                leftmat(1,1)=pdiff_plus.y;
                rightmat(0,0)=vpstart.x;
                rightmat(1,0)=vpstart.y;
                rightmat(0,1)=vpstart_plus.x;
                rightmat(1,1)=vpstart_plus.y;
                Rigid(i,j,index)=Weight(i,j,index)*leftmat*rightmat.t();
            }
        }
    }
    return true;
}
 
cv::MatMLSDRigid::Deformation(const std::vector<cv::Point2i>& dst_pt)
{
    if(n_point_ != dst_pt.size()) {
        return cv::Mat();
    }
    qi_point_.resize(n_point_);
    qdiff_.resize(n_point_);
    for (int i = 0; i<n_point_; ++i) {
        qi_point_[i]=dst_pt[i];
    }
    for(int i=0;; i+=gridsize_) {
        if (i>=src_width_ &&i<src_width_+gridsize_ - 1) { //计算最后一行一列
            i=src_width_-1;
        } else if (i>=src_width_) {
            break;
        }
        for (int j = 0; ; j+=gridsize_) {
            if (j>=src_height_ &&j<src_height_+gridsize_ - 1) { //计算最后一行一列
                j = src_height_ - 1;
            } else if (j>=src_height_) {
                break;
            }
            ///由qi计算qstart,qdiff
            bool isequalpi=false;
            cv::Point2d piex_pt(i,j);
            qstart_=cv::Point2d(0.,0.);
            for(int index=0; index<n_point_;++index) {
                if(pi_point_[index].x == i&& pi_point_[index].y ==j ) {
                    isequalpi=true;
                    break;
                }
               qstart_+=Weight(i,j,index)*qi_point_[index];
            }
            if(isequalpi) {
                DefPT(i,j)=cv::Point2d(0,0);
            } else {
                qstart_*=(1/SumWeight(i,j));
                for(int index=0;index<n_point_; ++index) {
                   qdiff_[index]=qi_point_[index]-qstart_;
                }
                ///fj=|v-pstart|*sum(qi*ai)/|sum(qi*ai|+qstart
                cv::Matsummat=cv::Mat::zeros(1,2,CV_64F);
                cv::MatpointMat(cv::Point2d(i,j)-RigidPStart(i,j));
                for(int index=0;index<n_point_; ++index) {
                    summat+=cv::Mat(qdiff_[index]).t()*cv::Mat(Rigid(i,j,index));
                }
               summat*=cv::norm(pointMat)/cv::norm(cv::Mat(summat));
                summat+=cv::Mat(qstart_).t();
               DefPT(i,j).x=summat.at<double>(0,0)-i;
                DefPT(i,j).y=summat.at<double>(0,1)-j;
            }
        }
    }
    return getDeformationImg();
}