自学教程：C++ svd函数代码示例

bool VersionManager::isResNeedUpdate(){	return svd().ver.is_high_to(rvd().ver);}

void ForecastMachine::smap_prediction(const size_t start, const size_t end){    size_t curr_pred, effective_nn, E = data_vectors[0].size();    double avg_distance;    vec weights;    std::vector<size_t> nearest_neighbors;    MatrixXd A, S_inv;    VectorXd B, S, x;    double max_s, pred;    std::vector<size_t> temp_lib;        for(size_t k = start; k < end; ++k)    {        curr_pred = which_pred[k];                // find nearest neighbors        if(CROSS_VALIDATION)        {            temp_lib = which_lib;            adjust_lib(curr_pred);            nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred));            which_lib = temp_lib;        }        else        {            nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred));        }        effective_nn = nearest_neighbors.size();                if(effective_nn == 0)        {            predicted[curr_pred] = qnan;            LOG_WARNING("no nearest neighbors found; using NA for forecast");            continue;        }                weights.assign(effective_nn, 1.0); // default is for theta = 0        if(theta > 0.0)        {            // compute average distance            avg_distance = 0;            for(auto& neighbor: nearest_neighbors)            {                avg_distance += distances.coeff(curr_pred, neighbor);            }            avg_distance /= effective_nn;                        // compute weights            for(size_t i = 0; i < effective_nn; ++i)                weights[i] = exp(-theta * distances.coeff(curr_pred, nearest_neighbors[i]) / avg_distance);        }                // setup matrices for SVD        A.resize(effective_nn, E+1);        B.resize(effective_nn);                for(size_t i = 0; i < effective_nn; ++i)        {            B(i) = weights[i] * targets[nearest_neighbors[i]];                        for(size_t j = 0; j < E; ++j)                A(i, j) = weights[i] * data_vectors[nearest_neighbors[i]][j];            A(i, E) = weights[i];        }                // perform SVD        JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);                // remove singular values close to 0        S = svd.singularValues();        S_inv = MatrixXd::Zero(E+1, E+1);        max_s = S(0) * 1e-5;        for(size_t j = 0; j <= E; ++j)        {            if(S(j) >= max_s)                S_inv(j, j) = 1/S(j);        }                // perform back-substitution to solve        x = svd.matrixV() * S_inv * svd.matrixU().transpose() * B;                pred = 0;        for(size_t j = 0; j < E; ++j)            pred += x(j) * data_vectors[curr_pred][j];        pred += x(E);        if(SAVE_SMAP_COEFFICIENTS)        {            for(size_t j = 0; j <= E; ++j)                smap_coefficients[curr_pred][j] = x(j);        }        // save prediction        predicted[curr_pred] = pred;    }    return;}

void HomographyModel::estimate_from_minimal_set(const vector< ScoredMatch> &data_points){ // given m points estimate the parameters vector    // based on vxl rrel_homography2d_est :: fit_from_minimal_set    if ( data_points.size() < get_num_points_to_estimate())        throw runtime_error("Not enough points to estimate the HomographyModel parameters");    vnl_matrix< double > A(9, 9, 0.0);    for ( int i=0; i < get_num_points_to_estimate(); i+=1 )    { // for i = 0,1,2,3        vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);        vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);        if (false)        { // just for debugging            printf("from_points[%i] -> to_points[%i] ==", i,i);            cout << from_point << " -> " << to_point << endl;        }        A( 2*i, 0 ) = A( 2*i+1, 3 ) = from_point.x() * to_point.w();        A( 2*i, 1 ) = A( 2*i+1, 4 ) = from_point.y() * to_point.w();        A( 2*i, 2 ) = A( 2*i+1, 5 ) = from_point.w() * to_point.w();        A( 2*i, 6 ) = -1 * from_point.x() * to_point.x();        A( 2*i, 7 ) = -1 * from_point.y() * to_point.x();        A( 2*i, 8 ) = -1 * from_point.w() * to_point.x();        A( 2*i+1, 6 ) = -1 * from_point.x() * to_point.y();        A( 2*i+1, 7 ) = -1 * from_point.y() * to_point.y();        A( 2*i+1, 8 ) = -1 * from_point.w() * to_point.y();    }    vnl_svd<double> svd( A, 1.0e-8 );    if (false)    { // just for debugging        cout << "A == " << A << endl;        cout << "svd(A) == " << svd << endl;    }    const unsigned int homog_dof_ = 8;    if ( svd.rank() < homog_dof_ )    {        // singular fit        if (true)        { // just for debugging            cout << "svd.rank() == " << svd.rank() << endl;            for ( unsigned int i=0; i<get_num_points_to_estimate(); ++i )            {                vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);                vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);                cout << "from->to point[i]-> " << from_point << "->" << to_point << endl;            }        }        throw runtime_error("HomographyModel::estimate_from_minimal_set failed");    }    vnl_vector<double> params = svd.nullvector();    parameters.resize(get_num_parameters());    if (params.size() != parameters.size() )    {        throw runtime_error("HomographyModel::estimate_from_minimal_set internal error");    }    for ( unsigned int i=0; i<get_num_parameters(); i+=1 )    {        parameters[i] = params[i]; // copy the result    }   if ( false ) {	    cout << "HomographyModel parameters: " << parameters << endl;   }       return;} // end of 'HomographyModel::estimate_from_minimal_set'

inlineboolop_princomp::direct_princomp  (         Mat< std::complex<typename T1::pod_type> >&     coeff_out,         Mat< std::complex<typename T1::pod_type> >&     score_out,         Col<              typename T1::pod_type  >&     latent_out,  const Base< std::complex<typename T1::pod_type>, T1 >& X,  const typename arma_cx_only<typename T1::elem_type>::result* junk  )  {  arma_extra_debug_sigprint();  arma_ignore(junk);    typedef typename T1::pod_type     T;  typedef          std::complex<T> eT;    const unwrap_check<T1> Y( X.get_ref(), score_out );  const Mat<eT>& in    = Y.M;    const uword n_rows = in.n_rows;  const uword n_cols = in.n_cols;    if(n_rows > 1) // more than one sample    {    // subtract the mean - use score_out as temporary matrix    score_out = in;  score_out.each_row() -= mean(in);        // singular value decomposition    Mat<eT> U;    Col< T> s;        const bool svd_ok = svd(U, s, coeff_out, score_out);        if(svd_ok == false)  { return false; }        // normalize the eigenvalues    s /= std::sqrt( double(n_rows - 1) );        // project the samples to the principals    score_out *= coeff_out;        if(n_rows <= n_cols) // number of samples is less than their dimensionality      {      score_out.cols(n_rows-1,n_cols-1).zeros();            Col<T> s_tmp = zeros< Col<T> >(n_cols);      s_tmp.rows(0,n_rows-2) = s.rows(0,n_rows-2);      s = s_tmp;      }          // compute the eigenvalues of the principal vectors    latent_out = s%s;    }  else // 0 or 1 samples    {    coeff_out.eye(n_cols, n_cols);    score_out.copy_size(in);    score_out.zeros();    latent_out.set_size(n_cols);    latent_out.zeros();    }    return true;  }

// Overloaded version:  Return singular values only.//---------------------------------------------------------DVec& svd(const DMat& mat) //---------------------------------------------------------{  DMat U, VT;    return svd(mat, U, VT, 'N', 'N'); }

ICP_API float __stdcall ICP(Point3f *verts1, Point3f *verts2, int nVerts1, int nVerts2, float *R, float *t, int maxIter){	PointCloud cloud1;	cloud1.pts = vector<Point3f>(verts1, verts1 + nVerts1);	cv::Mat matR(3, 3, CV_32F, R);	cv::Mat matT(1, 3, CV_32F, t);	cv::Mat verts2Mat(nVerts2, 3, CV_32F, (float*)verts2);	float error = 1;	for (int iter = 0; iter < maxIter; iter++)	{		vector<Point3f> matched1, matched2;		vector<float> distances(nVerts2);		vector<size_t> indices(nVerts2);		FindClosestPointForEach(cloud1, verts2Mat, distances, indices);		vector<float> matchDistances;		vector<int> matchIdxs(nVerts1, -1);		for (int i = 0; i < nVerts2; i++)		{			int pos = matchIdxs[indices[i]];			if (pos != -1)			{				if (matchDistances[pos] < distances[i])					continue;			}			Point3f temp;			temp.X = verts2Mat.at<float>(i, 0);			temp.Y = verts2Mat.at<float>(i, 1);			temp.Z = verts2Mat.at<float>(i, 2);			if (pos == -1)			{				matched1.push_back(verts1[indices[i]]);				matched2.push_back(temp);				matchDistances.push_back(distances[i]);				matchIdxs[indices[i]] = matched1.size() - 1;			}			else			{				matched2[pos] = temp;				matchDistances[pos] = distances[i];			}		}		RejectOutlierMatches(matched1, matched2, matchDistances, 2.5);		//error = 0;		//for (int i = 0; i < matchDistances.size(); i++)		//{		//	error += sqrt(matchDistances[i]);		//}		//error /= matchDistances.size();		//cout << error << endl;		cv::Mat matched1MatCv(matched1.size(), 3, CV_32F, matched1.data());		cv::Mat matched2MatCv(matched2.size(), 3, CV_32F, matched2.data());		cv::Mat tempT;		cv::reduce(matched1MatCv - matched2MatCv, tempT, 0, CV_REDUCE_AVG);		for (int i = 0; i < verts2Mat.rows; i++)		{			verts2Mat.row(i) += tempT;		}		for (int i = 0; i < matched2MatCv.rows; i++)		{			matched2MatCv.row(i) += tempT;		}		cv::Mat M = matched2MatCv.t() * matched1MatCv;		cv::SVD svd;		svd(M);		cv::Mat tempR = svd.u * svd.vt;		double det = cv::determinant(tempR);		if (det < 0)		{			cv::Mat temp = cv::Mat::eye(3, 3, CV_32F);			temp.at<float>(2, 2) = -1;			tempR = svd.u * temp * svd.vt;		}		verts2Mat = verts2Mat * tempR;		matT += tempT * matR.t();		matR = matR * tempR;	}	memcpy(verts2, verts2Mat.data, verts2Mat.rows * sizeof(float) * 3);	memcpy(R, matR.data, 9 * sizeof(float));	memcpy(t, matT.data, 3 * sizeof(float));//.........这里部分代码省略.........

inlinevoid r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points,             MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw,             int maxIterNum,float TerminateTh,int nargin){    if(nargin<6)    {        return;    }    if(nargin<8)    {        maxIterNum = 8;        TerminateTh = 1e-5;    }    int n = start_points.cols();    if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols())    {        return;    }    if(n<4)    {        return;    }    //first compute the weight of each line and the normal of    //the interpretation plane passing through to camera center and the line    VectorXf w = VectorXf::Zero(n);    MatrixXf nc = MatrixXf::Zero(3,n);    for(int i = 0 ; i < n ; i++)    {        //the weight of a line is the inverse of its image length        w[i] = 1/(start_points.col(i)-end_points.col(i)).norm();        vfloat3 v1 = start_points.col(i);        vfloat3 v2 = end_points.col(i);        vfloat3 temp = v1.cross(v2);        nc.col(i) = temp/temp.norm();    }    MatrixXf rot_wc = initPos_cw.transpose();    MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw;    for(int iter = 1 ; iter < maxIterNum ; iter++)    {        //construct the equation (31)        MatrixXf A = MatrixXf::Zero(6,7);        MatrixXf C = MatrixXf::Zero(3,3);        MatrixXf D = MatrixXf::Zero(3,3);        MatrixXf F = MatrixXf::Zero(3,3);        vfloat3 c_bar = vfloat3(0,0,0);        vfloat3 d_bar = vfloat3(0,0,0);        for(int i = 0 ; i < n ; i++)        {            //for first point on line            vfloat3 Pi = rot_wc * P1w.col(i);            vfloat3 Ni = nc.col(i);            float wi = w[i];            vfloat3 bi = Pi.cross(Ni);            C = C + wi*Ni*Ni.transpose();            D = D + wi*bi*bi.transpose();            F = F + wi*Ni*bi.transpose();            vfloat3 tempi = Pi + pos_wc;            float scale = Ni.transpose() * tempi;            scale *= wi;            c_bar = c_bar + scale * Ni;            d_bar = d_bar + scale*bi;            //for second point on line            Pi = rot_wc * P2w.col(i);            Ni = nc.col(i);            wi = w[i];            bi = Pi.cross(Ni);            C  = C + wi*Ni*Ni.transpose();            D  = D + wi*bi*bi.transpose();            F  = F + wi*Ni*bi.transpose();            scale = (Ni.transpose() * (Pi + pos_wc));            scale *= wi;            c_bar = c_bar + scale * Ni;            d_bar = d_bar + scale * bi;        }        A.block<3,3>(0,0) = C;        A.block<3,3>(0,3) = F;        (A.col(6)).segment(0,2) = c_bar;        A.block<3,3>(3,0) = F.transpose();        A.block<3,3>(2,2) = D;        (A.col(6)).segment(3,5) = d_bar;        //sovle the system by using SVD;        JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV);        VectorXf vec(7);        //the last column of Vmat;        vec = (svd.matrixV()).col(6);        //the condition that the last element of vec should be 1.        vec = vec/vec[6];        //update the rotation and translation parameters;//.........这里部分代码省略.........

//.........这里部分代码省略.........            for(int i = 0 ; i < n ; i++)            {                float nxi = (nc_bar[i])[0];                float nyi = (nc_bar[i])[1];                float nzi = (nc_bar[i])[2];                Eigen::VectorXf Vm(3);                Vm = RzRxRot * directions.col(i);                float Vxi = Vm[0];                float Vyi = Vm[1];                float Vzi = Vm[2];                Eigen::VectorXf Pm(3);                Pm = RzRxRot * points.col(i);                float Pxi = Pm(1);                float Pyi = Pm(2);                float Pzi = Pm(3);                //apply the constraint scalarproduct(Vi^c, ni^c) = 0                //if i=1, then scalarproduct(Vi^c, ni^c) always be 0                if(i>1)                {                    Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;                    Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;                    Matrice(2*i-3, 5) = nyi * Vyi;                }                //apply the constraint scalarproduct(Pi^c, ni^c) = 0                Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;                Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;                Matrice(2*i-2, 2) = -nxi;                Matrice(2*i-2, 3) = -nyi;                Matrice(2*i-2, 4) = -nzi;                Matrice(2*i-2, 5) = nyi * Pyi;            }            //solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0  using SVD,            JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);            Eigen::MatrixXf VMat = svd.matrixV();            Eigen::VectorXf vec(2*n-1);            //the last column of Vmat;            vec = VMat.col(5);            //the condition that the last element of vec should be 1.            vec = vec/vec[5];            //the condition costheta^2+sintheta^2 = 1;            float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);            float costheta = vec[0]*normalizeTheta;            float sintheta = vec[1]*normalizeTheta;            Eigen::MatrixXf Ry(3,3);            Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;            //now, we get the rotation matrix rot_wc and translation pos_wc            Eigen::MatrixXf rot_wc(3,3);            rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;            Eigen::VectorXf pos_wc(3);            pos_wc = - Rot.transpose() * vec.segment(2,4);            //now normalize the camera pose by 3D alignment. We first translate the points            //on line in the world frame Pw to points in the camera frame Pc. Then we project            //Pc onto the line interpretation plane as Pc_new. So we could call the point            //alignment algorithm to normalize the camera by aligning Pc_new and Pw.            //In order to improve the accuracy of the aligment step, we choose two points for each            //lines. The first point is Pwi, the second point is  the closest point on line i to camera center.            //(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)            Eigen::MatrixXf Pw2(3,n);            Pw2.setZero();            Eigen::MatrixXf Pc_new(3,n);            Pc_new.setZero();            Eigen::MatrixXf Pc2_new(3,n);            Pc2_new.setZero();

void Mesher::check_feature(){    auto contour = get_contour();    const auto normals = get_normals(contour);    // Find the largest cone and the normals that enclose    // the largest angle as n0, n1.    float theta = 1;    Vec3f n0, n1;    for (auto ni : normals)    {        for (auto nj : normals)        {            float dot = ni.dot(nj);            if (dot < theta)            {                theta = dot;                n0 = ni;                n1 = nj;            }        }    }    // If there isn't a feature in this fan, then return immediately.    if (theta > 0.9)        return;    // Decide whether this is a corner or edge feature.    const Vec3f nstar = n0.cross(n1);    float phi = 0;    for (auto n : normals)        phi = fmax(phi, fabs(nstar.dot(n)));    bool edge = phi < 0.7;    // Find the center of the contour.    Vec3f center(0, 0, 0);    for (auto c : contour)        center += c;    center /= contour.size();    // Construct the matrices for use in our least-square fit.    Eigen::MatrixX3d A(normals.size(), 3);    {        int i=0;        for (auto n : normals)            A.row(i++) << n.transpose();    }    // When building the second matrix, shift position values to be centered    // about the origin (because that's what the least-squares fit will    // minimize).    Eigen::VectorXd B(normals.size(), 1);    {        auto n = normals.begin();        auto c = contour.begin();        int i=0;        while (n != normals.end())            B.row(i++) << (n++)->dot(*(c++) - center);    }    // Use singular value decomposition to solve the least-squares fit.    Eigen::JacobiSVD<Eigen::MatrixX3d> svd(A, Eigen::ComputeFullU |                                              Eigen::ComputeFullV);    // Set the smallest singular value to zero to make fitting happier.    if (edge)    {        auto singular = svd.singularValues();        svd.setThreshold(singular.minCoeff() / singular.maxCoeff() * 1.01);    }    // Solve for the new point's position.    const Vec3f new_pt = svd.solve(B) + center;    // Erase this triangle fan, as we'll be inserting a vertex in the center.    triangles.erase(fan_start, voxel_start);    // Construct a new triangle fan.    contour.push_back(contour.front());    {        auto p0 = contour.begin();        auto p1 = contour.begin();        p1++;        while (p1 != contour.end())            push_swappable_triangle(Triangle(*(p0++), *(p1++), new_pt));    }}

inlinevoid cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw,                int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2){    //A    Eigen::MatrixXf X = XXw;    //B    Eigen::MatrixXf Y = XXc;    Eigen::MatrixXf eyen(n,n);    eyen = Eigen::MatrixXf::Identity(n,n);    Eigen::MatrixXf ones(n,n);    ones.setOnes();    Eigen::MatrixXf K(n,n);    K = eyen - ones/n;    vfloat3 ux;    for(int i =0; i < n; i++)    {        ux = ux + X.col(i);    }    ux = ux/n;    vfloat3 uy;    for(int i =0; i < n; i++)    {        uy = uy + Y.col(i);    }    uy = uy/n;    Eigen::MatrixXf XK(3,n);    XK = X*K;    Eigen::MatrixXf XKarre(3,n);    for(int i = 0 ; i < n ; i++)    {        XKarre(0,i) = XK(0,i)*XK(0,i);        XKarre(1,i) = XK(1,i)*XK(1,i);        XKarre(2,i) = XK(2,i)*XK(2,i);    }    Eigen::VectorXf sumXKarre(n);    float sigmx2 = 0;    for(int i = 0 ; i < n ; i++)    {        sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i);        sigmx2 += sumXKarre[i];    }    sigmx2 /=n;    Eigen::MatrixXf SXY(3,3);    SXY = Y*K*(X.transpose())/n;    JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV);    Eigen::MatrixXf S(3,3);    S = Eigen::MatrixXf::Identity(3,3);    if(SXY.determinant() < 0)    {        S(3,3) = -1;    }    R2 = svd.matrixU() * S * (svd.matrixV()).transpose();    Eigen::MatrixXf D(3,3);    D.setZero();    for(int i = 0 ; i < svd.singularValues().size() ; i++)    {        D(i,i) = (svd.singularValues())[i];    }    float c2 = (D*S).trace()/sigmx2;    t2 = uy - c2*R2*ux;    vfloat3 Xx = R2.col(0);    vfloat3 Yy = R2.col(1);    vfloat3 Zz = R2.col(2);    if((x_cross(Xx,Yy)-Zz).norm()>2e-2)    {        R2.col(2) = -Zz;    }}

void solveLinear(Double_t eps = 1.e-12){   cout << "Perform the fit  y = c0 + c1 * x in four different ways" << endl;   const Int_t nrVar  = 2;   const Int_t nrPnts = 4;   Double_t ax[] = {0.0,1.0,2.0,3.0};   Double_t ay[] = {1.4,1.5,3.7,4.1};   Double_t ae[] = {0.5,0.2,1.0,0.5};   // Make the vectors 'Use" the data : they are not copied, the vector data   // pointer is just set appropriately   TVectorD x; x.Use(nrPnts,ax);   TVectorD y; y.Use(nrPnts,ay);   TVectorD e; e.Use(nrPnts,ae);   TMatrixD A(nrPnts,nrVar);   TMatrixDColumn(A,0) = 1.0;   TMatrixDColumn(A,1) = x;   cout << " - 1. solve through Normal Equations" << endl;   const TVectorD c_norm = NormalEqn(A,y,e);   cout << " - 2. solve through SVD" << endl;   // numerically  preferred method   // first bring the weights in place   TMatrixD Aw = A;   TVectorD yw = y;   for (Int_t irow = 0; irow < A.GetNrows(); irow++) {      TMatrixDRow(Aw,irow) *= 1/e(irow);      yw(irow) /= e(irow);   }   TDecompSVD svd(Aw);   Bool_t ok;   const TVectorD c_svd = svd.Solve(yw,ok);   cout << " - 3. solve with pseudo inverse" << endl;   const TMatrixD pseudo1  = svd.Invert();   TVectorD c_pseudo1 = yw;   c_pseudo1 *= pseudo1;   cout << " - 4. solve with pseudo inverse, calculated brute force" << endl;   TMatrixDSym AtA(TMatrixDSym::kAtA,Aw);   const TMatrixD pseudo2 = AtA.Invert() * Aw.T();   TVectorD c_pseudo2 = yw;   c_pseudo2 *= pseudo2;   cout << " - 5. Minuit through TGraph" << endl;   TGraphErrors *gr = new TGraphErrors(nrPnts,ax,ay,0,ae);   TF1 *f1 = new TF1("f1","pol1",0,5);   gr->Fit("f1","Q");   TVectorD c_graph(nrVar);   c_graph(0) = f1->GetParameter(0);   c_graph(1) = f1->GetParameter(1);   // Check that all 4 answers are identical within a certain   // tolerance . The 1e-12 is somewhat arbitrary . It turns out that   // the TGraph fit is different by a few times 1e-13.   Bool_t same = kTRUE;   same &= VerifyVectorIdentity(c_norm,c_svd,0,eps);   same &= VerifyVectorIdentity(c_norm,c_pseudo1,0,eps);   same &= VerifyVectorIdentity(c_norm,c_pseudo2,0,eps);   same &= VerifyVectorIdentity(c_norm,c_graph,0,eps);   if (same)      cout << " All solutions are the same within tolerance of " << eps << endl;   else      cout << " Some solutions differ more than the allowed tolerance of " << eps << endl;}

//.........这里部分代码省略.........		{			v1 = GCI * pow(PI/mi->first->alpha,1.5);			mass1[0][0] += v1 * p1.point.x * p1.point.x;			mass1[0][1] += v1 * p1.point.x * p1.point.y;			mass1[0][2] += v1 * p1.point.x * p1.point.z;			mass1[1][1] += v1 * p1.point.y * p1.point.y;			mass1[1][2] += v1 * p1.point.y * p1.point.z;			mass1[2][2] += v1 * p1.point.z * p1.point.z;					v2 = GCI * pow(PI/mi->second->alpha,1.5);			mass2[0][0] += v2 * p2.point.x * p2.point.x;			mass2[0][1] += v2 * p2.point.x * p2.point.y;			mass2[0][2] += v2 * p2.point.x * p2.point.z;			mass2[1][1] += v2 * p2.point.y * p2.point.y;			mass2[1][2] += v2 * p2.point.y * p2.point.z;			mass2[2][2] += v2 * p2.point.z * p2.point.z;		}		// add new points to local maps		_refMap.push_back(p1);		_dbMap.push_back(p2);			}		// use SVD to compute best rotations	// set lower triangle	mass1[1][0] = mass1[0][1];	mass1[2][0] = mass1[0][2];	mass1[2][1] = mass1[1][2];		// normalize mass matrix	mass1 /= V1;   	// compute SVD of the mass matrix	SiMath::SVD svd(mass1, true, true);	_refRotMat = svd.getU();   	// check if determinant is 1, otherwise it is a mirroring instead of rotation	double det = _refRotMat[0][0]*_refRotMat[1][1]*_refRotMat[2][2]		+ _refRotMat[2][1]*_refRotMat[1][0]*_refRotMat[0][2]		+ _refRotMat[0][1]*_refRotMat[1][2]*_refRotMat[2][0] 		- _refRotMat[0][0]*_refRotMat[1][2]*_refRotMat[2][1]		- _refRotMat[1][1]*_refRotMat[2][0]*_refRotMat[0][2]		- _refRotMat[2][2]*_refRotMat[0][1]*_refRotMat[1][0];			// check if it is a rotation matrix and not a mirroring	if (det < 0)	{		// switch sign of third column		_refRotMat[0][2] = -_refRotMat[0][2];		_refRotMat[1][2] = -_refRotMat[1][2];		_refRotMat[2][2] = -_refRotMat[2][2];	}		// set lower triangle	mass2[1][0] = mass2[0][1];	mass2[2][0] = mass2[0][2];	mass2[2][1] = mass2[1][2];		// normalize mass matrix	mass2 /= V2;   	// compute SVD of the mass matrix	SiMath::SVD svd2(mass2, true, true);	_dbRotMat = svd2.getU();		// check if determinant is 1, otherwise it is a mirroring instead of rotation

//.........这里部分代码省略.........   for(k=0;k<m;k++){      for(l=0;l<m;l++){         if(k!=l){            E_mat[k][l]=-1.0/(double)m;         }         else{            E_mat[k][l]=1.0-1.0/(double)m;         }      }   }   if(debug)printf("E:/n");   if(debug)plot_matrix(stdout,  m, m, E_mat);   if(debug)printf("dest_mat_T:/n");   if(debug)plot_matrix(stdout,  n, m, dest_mat_T);   matmult(dest_mat_T, m, m, E_mat, m, m,  C_mat_interm, m, n);   if(debug)printf("C_interm:/n");   if(debug)plot_matrix(stdout,  n, m, C_mat_interm);   matmult(C_mat_interm, n, m, src_mat, m, n,  C_mat, n, n);   if(debug)printf("C:/n");   if(debug)plot_matrix(stdout,  n, n, C_mat);   copy_matrix(n,n,C_mat,P_mat);   if(debug)printf("P:/n");   if(debug)plot_matrix(stdout,  n, n, P_mat);   //Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n].   svd(n, n, C_mat, P_mat, D_vec, Q_mat);   transpose_matrix(n, n, P_mat, P_mat_T);   if(debug)printf("P/n");   if(debug)plot_matrix(stdout,  n, n, P_mat);   if(debug)printf("P_T/n");   if(debug)plot_matrix(stdout,  n, n, P_mat_T);   if(debug)printf("D_vec/n");   if(debug)plot_vector(stdout,  n, D_vec);   for(k=0;k<n;k++){      for(l=0;l<n;l++){         D_mat[k][l]=0.0;         D_mat[l][l]=D_vec[l];      }   }   if(debug)printf("D/n");   if(debug)plot_matrix(stdout,  n, n, D_mat);   matmult(Q_mat, n, n, P_mat_T, n, n,  R_mat, n, n);   if(debug)printf("R_trans:/n");   if(debug)plot_matrix(stdout, n, n, R_mat);   matmult(C_mat, m, n, R_mat, n, m,  C_mat_interm, m, n);   if(debug)printf("C_interm:/n");   if(debug)plot_matrix(stdout,  n, n, C_mat_interm);   trace1=trace(n,n,C_mat_interm);   if(debug)printf("/ntra=%lf/n/n",trace1);

void LocalLinearLeastSquaresExtrapolator::extrapolateElement(    std::size_t const element_index,    const unsigned num_components,    ExtrapolatableElementCollection const& extrapolatables,    const double t,    GlobalVector const& current_solution,    LocalToGlobalIndexMap const& dof_table,    GlobalVector& counts){    auto const& integration_point_values =        extrapolatables.getIntegrationPointValues(            element_index, t, current_solution, dof_table,            _integration_point_values_cache);    auto const& N_0 = extrapolatables.getShapeMatrix(element_index, 0);    auto const num_nodes = static_cast<unsigned>(N_0.cols());    auto const num_values =        static_cast<unsigned>(integration_point_values.size());    if (num_values % num_components != 0)        OGS_FATAL(            "The number of computed integration point values is not divisable "            "by the number of num_components. Maybe the computed property is "            "not a %d-component vector for each integration point.",            num_components);    // number of integration points in the element    const auto num_int_pts = num_values / num_components;    if (num_int_pts < num_nodes)        OGS_FATAL(            "Least squares is not possible if there are more nodes than"            "integration points.");    auto const pair_it_inserted = _qr_decomposition_cache.emplace(        std::make_pair(num_nodes, num_int_pts), CachedData{});    auto& cached_data = pair_it_inserted.first->second;    if (pair_it_inserted.second)    {        DBUG("Computing new singular value decomposition");        // interpolation_matrix * nodal_values = integration_point_values        // We are going to pseudo-invert this relation now using singular value        // decomposition.        auto& interpolation_matrix = cached_data.A;        interpolation_matrix.resize(num_int_pts, num_nodes);        interpolation_matrix.row(0) = N_0;        for (unsigned int_pt = 1; int_pt < num_int_pts; ++int_pt)        {            auto const& shp_mat =                extrapolatables.getShapeMatrix(element_index, int_pt);            assert(shp_mat.cols() == num_nodes);            // copy shape matrix to extrapolation matrix row-wise            interpolation_matrix.row(int_pt) = shp_mat;        }        // JacobiSVD is extremely reliable, but fast only for small matrices.        // But we usually have small matrices and we don't compute very often.        // Cf.        // http://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html        //        // Decomposes interpolation_matrix = U S V^T.        Eigen::JacobiSVD<Eigen::MatrixXd> svd(            interpolation_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);        auto const& S = svd.singularValues();        auto const& U = svd.matrixU();        auto const& V = svd.matrixV();        // Compute and save the pseudo inverse V * S^{-1} * U^T.        auto const rank = svd.rank();        assert(rank == num_nodes);        // cf. http://eigen.tuxfamily.org/dox/JacobiSVD_8h_source.html        cached_data.A_pinv.noalias() = V.leftCols(rank) *                                       S.head(rank).asDiagonal().inverse() *                                       U.leftCols(rank).transpose();    }    else if (cached_data.A.row(0) != N_0)    {        OGS_FATAL("The cached and the passed shapematrices differ.");    }    auto const& global_indices =        _dof_table_single_component(element_index, 0).rows;    if (num_components == 1)    {        auto const integration_point_values_vec =            MathLib::toVector(integration_point_values);        // Apply the pre-computed pseudo-inverse.        Eigen::VectorXd const nodal_values =            cached_data.A_pinv * integration_point_values_vec;        // TODO does that give rise to PETSc problems? E.g., writing to ghost        // nodes? Furthermore: Is ghost nodes communication necessary for PETSc?//.........这里部分代码省略.........

//.........这里部分代码省略.........                for (int i = 0; i < labelIndices.size(); ++i)        {            if (ComputePointsBetweenEdges_v1(vec2, labelIndices[i], labelEdge))            {                labelVertices.push_back(labelEdge.a);                labelVertices.push_back(labelEdge.b);            }        }                /*Plane Fitting with SVD */        Eigen::Vector3f vor_c(0, 0, 0);        Eigen::Vector3f c(0, 0, 0);        Eigen::MatrixXf matA(3, labelVertices.size());                // calculate c        for (int i = 0; i < labelVertices.size(); ++i)        {            vor_c += labelVertices[i];        }                c = vor_c / labelVertices.size();                //fill the Matrix        for (int i = 0; i < labelVertices.size(); i++)        {            float xB, yB, zB;            xB = labelVertices[i].x() - c.x();            yB = labelVertices[i].y() - c.y();            zB = labelVertices[i].z() - c.z();            matA.col(i) << xB, yB, zB;        }                Eigen::JacobiSVD<Eigen::MatrixXf> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV);        Eigen::Vector3f planeNormal(0, 0, 0);        planeNormal = svd.matrixU().col(2);        float d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());                /*Begin Normal direction of the plane*/        std::vector<Eigen::Vector3f> halleVerticesSidePLane;        std::vector<double> disthalleVerticesSidePLane;        int indexHalleV;               Eigen::Vector3f halle_cVector;                        //alle halle knoten hinzufügen        for(int i = 0; i < labelIndices.size(); ++i)        {            if(vec2[labelIndices[i].i[0]].c >= 0)                halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[0]].p);                        if(vec2[labelIndices[i].i[1]].c >= 0)                halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[1]].p);                        if(vec2[labelIndices[i].i[2]].c >= 0)                halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[2]].p);                    }                for(int i = 0; i < halleVerticesSidePLane.size(); ++i)        {            disthalleVerticesSidePLane.push_back( Dist2Plane(halleVerticesSidePLane[i], planeNormal, d));        }                indexHalleV = *std::max_element(disthalleVerticesSidePLane.begin(), disthalleVerticesSidePLane.end());

Foam::label Foam::quadraticFitSnGradData::calcFit(    const List<point>& C,    const label faci){    vector idir(1,0,0);    vector jdir(0,1,0);    vector kdir(0,0,1);    findFaceDirs(idir, jdir, kdir, mesh(), faci);    scalarList wts(C.size(), scalar(1));    wts[0] = centralWeight_;    wts[1] = centralWeight_;    point p0 = mesh().faceCentres()[faci];    scalar scale = 0;    // calculate the matrix of the polynomial components    scalarRectangularMatrix B(C.size(), minSize_, scalar(0));    for(label ip = 0; ip < C.size(); ip++)    {        const point& p = C[ip];        scalar px = (p - p0)&idir;        scalar py = (p - p0)&jdir;        #ifdef SPHERICAL_GEOMETRY            scalar pz = mag(p) - mag(p0);        #else            scalar pz = (p - p0)&kdir;        #endif        if (ip == 0) scale = max(max(mag(px), mag(py)), mag(pz));        px /= scale;        py /= scale;        pz /= scale;        label is = 0;        B[ip][is++] = wts[0]*wts[ip];        B[ip][is++] = wts[0]*wts[ip]*px;        B[ip][is++] = wts[ip]*sqr(px);        if (dim_ >= 2)        {            B[ip][is++] = wts[ip]*py;            B[ip][is++] = wts[ip]*px*py;            B[ip][is++] = wts[ip]*sqr(py);        }        if (dim_ == 3)        {            B[ip][is++] = wts[ip]*pz;            B[ip][is++] = wts[ip]*px*pz;            //B[ip][is++] = wts[ip]*py*pz;            B[ip][is++] = wts[ip]*sqr(pz);        }    }    // Set the fit    label stencilSize = C.size();    fit_[faci].setSize(stencilSize);    scalarList singVals(minSize_);    label nSVDzeros = 0;    const scalar& deltaCoeff = mesh().deltaCoeffs()[faci];    bool goodFit = false;    for(int iIt = 0; iIt < 10 && !goodFit; iIt++)    {        SVD svd(B, SMALL);        scalar fit0 = wts[0]*wts[0]*svd.VSinvUt()[1][0]/scale;        scalar fit1 = wts[0]*wts[1]*svd.VSinvUt()[1][1]/scale;        goodFit =            fit0 < 0 && fit1 > 0         && mag(fit0 + deltaCoeff) < 0.5*deltaCoeff         && mag(fit1 - deltaCoeff) < 0.5*deltaCoeff;        if (goodFit)        {            fit_[faci][0] = fit0;            fit_[faci][1] = fit1;            for(label i = 2; i < stencilSize; i++)            {                fit_[faci][i] = wts[0]*wts[i]*svd.VSinvUt()[1][i]/scale;            }            singVals = svd.S();            nSVDzeros = svd.nZeros();        }        else // (not good fit so increase weight in the centre and for linear)        {            wts[0] *= 10;            wts[1] *= 10;            for(label i = 0; i < B.n(); i++)            {                B[i][0] *= 10;//.........这里部分代码省略.........

//.........这里部分代码省略.........         {            Matrix4x4d q = Q[i] + ZQ1[i];            if (i > 0)               projectConvHull_SO3(q);            else            {               makeZeroMatrix(q);               q[0][0] = 1;            }            Q1[i] = q;            addMatricesIP(Q[i] - q, ZQ1[i]);         } // end for (i)         // Shrinkage of T (we want to minimize T)         T2 = std::max(0.0, T + ZT2 - gamma);         ZT2 += T - T2;         // Cone constraint         for (int k = 0; k < nRelPoses; ++k)         {            double t = T + ZT1[k];            Matrix3x3d a = A[k] + ZA1[k];            proxDataResidual_Frobenius(sigma, t, a);            T1[k] = t;            ZT1[k] += T - t;            A1[k] = a;            addMatricesIP(A[k] - a, ZA1[k]);         } // end for (k)         // Enforce linear consistency         for (int k = 0; k < nRelPoses; ++k)         {            int const i = viewPairs[k].first;            int const j = viewPairs[k].second;            Matrix4x4d qi = Q[i] + ZQ2i[k];            Matrix4x4d qj = Q[j] + ZQ2j[k];            Matrix3x3d a = A[k] + ZA2[k];            proxConsistency(relativeRotations[k], qi, qj, a);            Q2i[k] = qi;            Q2j[k] = qj;            A2[k] = a;            addMatricesIP(Q[i] - qi, ZQ2i[k]);            addMatricesIP(Q[j] - qj, ZQ2j[k]);            addMatricesIP(A[k] - a, ZA2[k]);         } // end for (k)         // Averaging of the solutions         for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];         T = T2 - ZT2;         for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k];         T *= denomT;         T = std::max(0.0, T);         for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];         for (int k = 0; k < nRelPoses; ++k)         {            int const i = viewPairs[k].first;            int const j = viewPairs[k].second;            addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);            addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);            addMatricesIP(A2[k] - ZA2[k], A[k]);         } // end for (k)         for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);         for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);         if ((iter % 500) == 0)         {            cout << "iter: " << iter << " t = " << T << endl;            cout << "T1 = "; displayVector(T1);            cout << "ZT1 = "; displayVector(ZT1);            cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl;            Matrix<double> ZZ(4, 4);            for (int i = 0; i < nViews; ++i)            {               copyMatrix(Q[i], ZZ);               SVD<double> svd(ZZ);               cout << "Q = "; displayMatrix(ZZ);               cout << "SV = "; displayVector(svd.getSingularValues());               //Matrix3x3d R = getRotationFromQuat(Q[i]);               //cout << "R = "; displayMatrix(R);            } // end for (i)         }      } // end for (iter)      rotations.resize(nViews);      for (int i = 0; i < nViews; ++i)         rotations[i] = getRotationFromQuat(Q[i]);      zs = ZT1;   } // end computeConsistentRotations_Linf()

// Norm: Return largest singular value for M-by-N matrix.//---------------------------------------------------------double norm(const DMat& mat) //---------------------------------------------------------{  DVec s( svd(mat) );  return s(1); }

/** * @brief estimateHomography estimates an homography matrix H between image 1 to image 2 * @param points0 is an array of points computed from image 1. * @param points1 is an array of points computed from image 2. * @return It returns the homography matrix H. */PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0,                                   std::vector< Eigen::Vector2f > &points1){    Eigen::Matrix3d  H;    if((points0.size() != points1.size()) || (points0.size() < 4)) {        H.setZero();        return H;    }    Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);    Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);    Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);    Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);    int n = int(points0.size());    Eigen::MatrixXd A(n * 2, 9);    //set up the linear system    for(int i = 0; i < n; i++) {        //transform coordinates for increasing stability of the system        Eigen::Vector2f p0 = points0[i];        Eigen::Vector2f p1 = points1[i];        p0[0] = (p0[0] - transform_0[0]) / transform_0[2];        p0[1] = (p0[1] - transform_0[1]) / transform_0[2];        p1[0] = (p1[0] - transform_1[0]) / transform_1[2];        p1[1] = (p1[1] - transform_1[1]) / transform_1[2];        int j = i * 2;        A(j, 0) = 0.0;        A(j, 1) = 0.0;        A(j, 2) = 0.0;        A(j, 3) = p0[0];        A(j, 4) = p0[1];        A(j, 5) = 1.0;        A(j, 6) = -p1[1] * p0[0];        A(j, 7) = -p1[1] * p0[1];        A(j, 8) = -p1[1];        j++;        A(j, 0) = p0[0];        A(j, 1) = p0[1];        A(j, 2) = 1.0;        A(j, 3) = 0.0;        A(j, 4) = 0.0;        A(j, 5) = 0.0;        A(j, 6) = -p1[0] * p0[0];        A(j, 7) = -p1[0] * p0[1];        A(j, 8) = -p1[0];    }    //solve the linear system    Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);    Eigen::MatrixXd V = svd.matrixV();    n = int(V.cols()) - 1;    //assign and transpose    H(0, 0) = V(0, n);    H(0, 1) = V(1, n);    H(0, 2) = V(2, n);    H(1, 0) = V(3, n);    H(1, 1) = V(4, n);    H(1, 2) = V(5, n);    H(2, 0) = V(6, n);    H(2, 1) = V(7, n);    H(2, 2) = V(8, n);    H = mat_1.inverse() * H * mat_0;    return H / H(2, 2);}

bool Homography::decompose(){  decompositions.clear();  JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);  Vector3d singular_values = svd.singularValues();  double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)  double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.  double d3 = fabs(singular_values[2]);  Matrix3d U = svd.matrixU();  Matrix3d V = svd.matrixV();                    // VT^T  double s = U.determinant() * V.determinant();  double dPrime_PM = d2;  int nCase;  if(d1 != d2 && d2 != d3)    nCase = 1;  else if( d1 == d2 && d2 == d3)    nCase = 3;  else    nCase = 2;  if(nCase != 1)  {    printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");    return false;  }  double x1_PM;  double x2;  double x3_PM;  // All below deals with the case = 1 case.  // Case 1 implies (d1 != d3)  { // Eq. 12    x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));    x2    = 0;    x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));  };  double e1[4] = {1.0,-1.0, 1.0,-1.0};  double e3[4] = {1.0, 1.0,-1.0,-1.0};  Vector3d np;  HomographyDecomposition decomp;  // Case 1, d' > 0:  decomp.d = s * dPrime_PM;  for(size_t signs=0; signs<4; signs++)  {    // Eq 13    decomp.R = Matrix3d::Identity();    double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;    double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;    decomp.R(0,0) = dCosTheta;    decomp.R(0,2) = -dSinTheta;    decomp.R(2,0) = dSinTheta;    decomp.R(2,2) = dCosTheta;    // Eq 14    decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];    decomp.t[1] = 0.0;    decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];    np[0] = x1_PM * e1[signs];    np[1] = x2;    np[2] = x3_PM * e3[signs];    decomp.n = V * np;    decompositions.push_back(decomp);  }  // Case 1, d' < 0:  decomp.d = s * -dPrime_PM;  for(size_t signs=0; signs<4; signs++)  {    // Eq 15    decomp.R = -1 * Matrix3d::Identity();    double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;    double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;    decomp.R(0,0) = dCosPhi;    decomp.R(0,2) = dSinPhi;    decomp.R(2,0) = dSinPhi;    decomp.R(2,2) = -dCosPhi;    // Eq 16    decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];    decomp.t[1] = 0.0;    decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];    np[0] = x1_PM * e1[signs];    np[1] = x2;    np[2] = x3_PM * e3[signs];    decomp.n = V * np;//.........这里部分代码省略.........

void Foam::CentredFitSnGradData<Polynomial>::calcFit(    scalarList& coeffsi,    const List<point>& C,    const scalar wLin,    const scalar deltaCoeff,    const label facei){    vector idir(1,0,0);    vector jdir(0,1,0);    vector kdir(0,0,1);    this->findFaceDirs(idir, jdir, kdir, facei);    // Setup the point weights    scalarList wts(C.size(), scalar(1));    wts[0] = this->centralWeight();    wts[1] = this->centralWeight();    // Reference point    point p0 = this->mesh().faceCentres()[facei];    // p0 -> p vector in the face-local coordinate system    vector d;    // Local coordinate scaling    scalar scale = 1;    // Matrix of the polynomial components    scalarRectangularMatrix B(C.size(), this->minSize(), scalar(0));    forAll(C, ip)    {        const point& p = C[ip];        const vector p0p = p - p0;        d.x() = p0p & idir;        d.y() = p0p & jdir;        d.z() = p0p & kdir;        if (ip == 0)        {            scale = cmptMax(cmptMag((d)));        }        // Scale the radius vector        d /= scale;        Polynomial::addCoeffs(B[ip], d, wts[ip], this->dim());    }    // Additional weighting for constant and linear terms    for (label i = 0; i < B.m(); i++)    {        B(i, 0) *= wts[0];        B(i, 1) *= wts[0];    }    // Set the fit    label stencilSize = C.size();    coeffsi.setSize(stencilSize);    bool goodFit = false;    for (int iIt = 0; iIt < 8 && !goodFit; iIt++)    {        SVD svd(B, small);        scalarRectangularMatrix invB(svd.VSinvUt());        for (label i=0; i<stencilSize; i++)        {            coeffsi[i] = wts[1]*wts[i]*invB(1, i)/scale;        }        goodFit =        (            mag(wts[0]*wts[0]*invB(0, 0) - wLin)          < this->linearLimitFactor()*wLin)         && (mag(wts[0]*wts[1]*invB(0, 1) - (1 - wLin)        ) < this->linearLimitFactor()*(1 - wLin))         && coeffsi[0] < 0 && coeffsi[1] > 0         && mag(coeffsi[0] + deltaCoeff) < 0.5*deltaCoeff         && mag(coeffsi[1] - deltaCoeff) < 0.5*deltaCoeff;        if (!goodFit)        {            // (not good fit so increase weight in the centre and weight            //  for constant and linear terms)            WarningInFunction                << "Cannot fit face " << facei << " iteration " << iIt                << " with sum of weights " << sum(coeffsi) << nl                << "    Weights " << coeffsi << nl                << "    Linear weights " << wLin << " " << 1 - wLin << nl                << "    deltaCoeff " << deltaCoeff << nl                << "    sing vals " << svd.S() << nl                << "Components of goodFit:/n"                << "    wts[0]*wts[0]*invB(0, 0) = "                << wts[0]*wts[0]*invB(0, 0) << nl                << "    wts[0]*wts[1]*invB(0, 1) = "                << wts[0]*wts[1]*invB(0, 1)//.........这里部分代码省略.........

int main(void){    // P2    unsigned int width(300), height(300);    kn::ioEPS eps(width, height);    //Draw axis    eps.drawLine(0, height/2, width, height/2, 0.5, 0.5, 0.5, 1);    eps.drawLine(width/2, 0, width/2, height, 0.5, 0.5, 0.5, 1);        unsigned int nbPoints(100);#if 1      Eigen::MatrixXd pointsP2(nbPoints,3);    srand(time(NULL));        for(unsigned int i=0; i < nbPoints; ++i){        double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0;        double naturalOffset(10.0);        double randomOffset = (rand()%50) - 25.0;                pointsP2(i, 0) = x;        pointsP2(i, 1) = 0.8*x + naturalOffset + randomOffset;        pointsP2(i, 2) = 1.0;    }     //std::cout << " input data/n" << pointsP2 << std::endl << std::endl;        Eigen::Matrix3d signP2 = Eigen::Matrix3d::Identity();    signP2(1, 1) = -1.0;    //std::cout << " signP2/n" << signP2 << std::endl << std::endl;            Eigen::MatrixXd tmp(pointsP2);    tmp = pointsP2 * signP2;    std::cout << " input data/n" << tmp << std::endl << std::endl;        //SVD    Eigen::JacobiSVD<Eigen::MatrixXd> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);    Eigen::VectorXd h = svd.matrixV().rightCols(1);    std::cout << " Solution/n" << h/h(1) << std::endl;        double graduate(h(0)/h(1));    double offset = h(2)/h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1)));    //double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1)));        std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl;#else    Eigen::MatrixXd pointsP2(nbPoints,2);    Eigen::VectorXd yP2(nbPoints);    srand(time(NULL));        for(unsigned int i=0; i < nbPoints; ++i){        double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0;        double naturalOffset(10.0);        double randomOffset = (rand()%50) - 25.0;                pointsP2(i, 0) = x;        pointsP2(i, 1) = 1.0;        yP2(i) = 0.8*x + naturalOffset + randomOffset;    }     std::cout << " input data/n" << pointsP2 << std::endl << std::endl;          //SVD    Eigen::JacobiSVD<Eigen::MatrixXd> svd(pointsP2, Eigen::ComputeFullU | Eigen::ComputeFullV);    Eigen::VectorXd h = svd.solve(yP2);    std::cout << " Solution/n" << h << std::endl;        double graduate(h(0));    double offset = h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1)));    //double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1)));        std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl;#endif                                // affichage des points et droites //////////////////////////////////////////////////////////////////        double xTop   = ( ( double(height) / 2.0f) - offset ) / graduate;    double xBot   = ( (-double(height) / 2.0f) - offset ) / graduate;    double yLeft  = graduate * (- double(width)/2.0f ) + offset;    double yRight = graduate * (  double(width)/2.0f ) + offset;        bool alreadyIntersect(false);      double linePoint1x, linePoint1y;    double linePoint2x, linePoint2y;        if( (xTop >= (- double(width)/2.0f)) && (xTop <= double(width)/2.0f)){        std::cout << "Top" << std::endl;        linePoint1x = xTop;        linePoint1y = double(height)/2.0f;        alreadyIntersect = true;    }        if( (xBot >= (- double(width)/2.0f)) && (xBot <= double(width)/2.0f) ){        std::cout << "Bot" << std::endl;        if(alreadyIntersect){            linePoint2x = xBot;            linePoint2y = -double(height)/2.0f;//.........这里部分代码省略.........

/* *  Vertex positioning is based on [Kobbelt et al, 2001] */float Octree::findVertex(Evaluator* e){    findIntersections(e);    // Find the center of intersection positions    glm::vec3 center = std::accumulate(            intersections.begin(), intersections.end(), glm::vec3(),            [](const glm::vec3& a, const Intersection& b)                { return a + b.pos; }) / float(intersections.size());    /*  The A matrix is of the form     *  [n1x, n1y, n1z]     *  [n2x, n2y, n2z]     *  [n3x, n3y, n3z]     *  ...     *  (with one row for each Hermite intersection)     */    Eigen::MatrixX3f A(intersections.size(), 3);    for (unsigned i=0; i < intersections.size(); ++i)    {        auto d = intersections[i].norm;        A.row(i) << Eigen::Vector3f(d.x, d.y, d.z).transpose();    }    /*  The B matrix is of the form     *  [p1 . n1]     *  [p2 . n2]     *  [p3 . n3]     *  ...     *  (with one row for each Hermite intersection)     *     *  Positions are pre-emtively shifted so that the center of the contoru     *  is at 0, 0, 0 (since the least-squares fix minimizes distance to the     *  origin); we'll unshift afterwards.     */    Eigen::VectorXf B(intersections.size(), 1);    for (unsigned i=0; i < intersections.size(); ++i)    {        B.row(i) << glm::dot(intersections[i].norm,                             intersections[i].pos - center);    }    // Use singular value decomposition to solve the least-squares fit.    Eigen::JacobiSVD<Eigen::MatrixX3f> svd(A, Eigen::ComputeFullU |                                              Eigen::ComputeFullV);    // Truncate singular values below 0.1    auto singular = svd.singularValues();    svd.setThreshold(0.1 / singular.maxCoeff());    rank = svd.rank();    // Solve the equation and convert back to cell coordinates    Eigen::Vector3f solution = svd.solve(B);    vert = glm::vec3(solution.x(), solution.y(), solution.z()) + center;    // Clamp vertex to be within the bounding box    vert.x = std::min(X.upper(), std::max(vert.x, X.lower()));    vert.y = std::min(Y.upper(), std::max(vert.y, Y.lower()));    vert.z = std::min(Z.upper(), std::max(vert.z, Z.lower()));    // Find and return QEF residual    auto m = A * solution - B;    return m.transpose() * m;}

 at::Mat homographyToPose(at::real fx, at::real fy,                           at::real tagSize,                          const at::Mat& horig,                          bool openGLStyle) {       // flip the homography along the Y axis to align the   // conventional image coordinate system (y=0 at the top) with   // the conventional camera coordinate system (y=0 at the   // bottom).   at::Mat h = horig;   at::Mat F = at::Mat::eye(3,3);   F(1,1) = F(2,2) = -1;   h = F * horig;   at::Mat M(4,4);   M(0,0) =  h(0,0) / fx;   M(0,1) =  h(0,1) / fx;   M(0,3) =  h(0,2) / fx;   M(1,0) =  h(1,0) / fy;   M(1,1) =  h(1,1) / fy;   M(1,3) =  h(1,2) / fy;   M(2,0) =  h(2,0);   M(2,1) =  h(2,1);   M(2,3) =  h(2,2);   // Compute the scale. The columns of M should be made to be   // unit vectors. This is over-determined, so we take the   // geometric average.   at::real scale0 = sqrt(sq(M(0,0)) + sq(M(1,0)) + sq(M(2,0)));   at::real scale1 = sqrt(sq(M(0,1)) + sq(M(1,1)) + sq(M(2,1)));   at::real scale = sqrt(scale0*scale1);       M *= 1.0/scale;   // recover sign of scale factor by noting that observations must   // occur in front of the camera.   if (M(2,3) > 0) {     M *= -1;   }   // The bottom row should always be [0 0 0 1].     M(3,0) = 0;   M(3,1) = 0;   M(3,2) = 0;   M(3,3) = 1;   // recover third rotation vector by crossproduct of the other two   // rotation vectors   at::Vec3 a( M(0,0), M(1,0), M(2,0) );   at::Vec3 b( M(0,1), M(1,1), M(2,1) );   at::Vec3 ab = a.cross(b);       M(0,2) = ab[0];   M(1,2) = ab[1];   M(2,2) = ab[2];   // pull out just the rotation component so we can normalize it.   at::Mat R(3,3);   for (int i=0; i<3; ++i) {     for (int j=0; j<3; ++j) {       R(i,j) = M(i,j);     }   }   // polar decomposition, R = (UV')(VSV')   cv::SVD svd(R);   at::Mat MR = svd.u * svd.vt;      if (!openGLStyle) { MR = F * MR; }   for (int i=0; i<3; ++i) {     for (int j=0; j<3; ++j) {       M(i,j) = MR(i,j);     }   }   // Scale the results based on the scale in the homography. The   // homography assumes that tags span from -1 to +1, i.e., that   // they are two units wide (and tall).   for (int i = 0; i < 3; i++) {     at::real scl = openGLStyle ? 1 : F(i,i);     M(i,3) *= scl * tagSize / 2;   }       return M;    }

// ****************************************************************************// This function finds a set of vectors (P) such that, for each vector p:// A.p = 0// Note that A is a matrix of size rows x cols, P is a matrix of size cols x cols whose ROWS are the p vectors.// P must NOT point to any allocated memory prior to calling this function. // The rank deficit (nullity) is returned (0 if matrix is full rank, 1 if matrix has one singular value, etc).// tol defines how small a singular value must be in order to be considered a "zero". // ****************************************************************************long int find_null_space_vectors(double *A, long int rows, long int cols, double **P, double tolerance){	double *U, *D, *V, *R, max_val, min_val;	long int i, rank_deficit, c, r;	U=(double *)malloc(sizeof(double)*rows*cols);	if (U==NULL) quit_error((char*)"Out of memory");	D=(double *)malloc(sizeof(double)*cols*cols);	if (D==NULL) quit_error((char*)"Out of memory");	V=(double *)malloc(sizeof(double)*cols*cols);	if (V==NULL) quit_error((char*)"Out of memory");		// Calculate the Singular Value Decomposition	svd(A, rows, cols, U, D, V);	// Find the maximum and minimum magnitude singular values	max_val=0.0;	min_val=DBL_MAX;	for (i=0; i<cols; i++)	{		if (fabs(D[i*cols+i])<fabs(min_val)) min_val=fabs(D[i*cols+i]);		if (fabs(D[i*cols+i])>fabs(max_val)) max_val=fabs(D[i*cols+i]);	}	// Count the rank deficit of the matrix, and extract	// relevant rows of the transpose(V) matrix. 	rank_deficit=0;	for (i=0; i<cols; i++)	{		if (fabs(D[i*cols+i])<tolerance) rank_deficit++;	}		// Transpose V	matrix_transpose(V, cols, cols);	// Generate a reduced version of transpose(V)	R=(double *)malloc(sizeof(double)*cols*(rank_deficit));	if (R==NULL) quit_error((char*)"Out of memory");	r=0;	for (i=0; i<cols; i++)	{		if (fabs(D[i*cols+i])<fabs(tolerance))		{			for (c=0; c<cols; c++)			{				R[r*cols+c]=V[i*cols+c];			}			r++;		}	}	// Row reduce R	matrix_row_reduce(R, rank_deficit, cols, tolerance);	*P=R;		free(V);	free(D);	free(U);		return rank_deficit;}

//.........这里部分代码省略.........      model_coefficients = new_model_coefficients;      inlier_indices = new_inlier_indices;    }    for(size_t i=0; i < model_coefficients.size(); i++)    {      PlaneModel<PointT> pm;      pm.coefficients_ = model_coefficients[i];      pm.cloud_ = input_;      pm.inliers_ = inlier_indices[i];      //recompute coefficients based on distance to camera and normal?      Eigen::Vector4f centroid;      pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);      Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);      Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);      float sum_w = 0.f;      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)      {          const int &idx = inlier_indices[i].indices[k];          float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();          float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);          //w_k = 1.f;          M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);          sum_w += w_k;      }      Eigen::Matrix3f scatter;      scatter.setZero ();      scatter = M_w.transpose() * M_w;      Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);      //std::cout << svd.matrixV() << std::endl;      Eigen::Vector3f n = svd.matrixV().col(2);      //flip normal if required      if(n.dot(c*-1) < 0)          n = n * -1.f;      float d = n.dot(c) * -1.f;      //std::cout << "normal:" << n << std::endl;      //std::cout << "d:" << d << std::endl;      pm.coefficients_.values[0] = n[0];      pm.coefficients_.values[1] = n[1];      pm.coefficients_.values[2] = n[2];      pm.coefficients_.values[3] = d;      pcl::PointIndices clean_inlier_indices;      float dist_threshold_ = 0.01f;      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)      {          const int &idx = inlier_indices[i].indices[k];          Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();          float val = n.dot(p) + d;          if(std::abs(val) <= dist_threshold_)              clean_inlier_indices.indices.push_back( idx );      }      pm.inliers_ = clean_inlier_indices;      models_.push_back(pm);    }

void EM::mStep(){    // Update means_k, covs_k and weights_k from probs_ik    int dim = trainSamples.cols;    // Update weights    // not normalized first    reduce(trainProbs, weights, 0, CV_REDUCE_SUM);    // Update means    means.create(nclusters, dim, CV_64FC1);    means = Scalar(0);    const double minPosWeight = trainSamples.rows * DBL_EPSILON;    double minWeight = DBL_MAX;    int minWeightClusterIndex = -1;    for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)    {        if(weights.at<double>(clusterIndex) <= minPosWeight)            continue;        if(weights.at<double>(clusterIndex) < minWeight)        {            minWeight = weights.at<double>(clusterIndex);            minWeightClusterIndex = clusterIndex;        }        Mat clusterMean = means.row(clusterIndex);        for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++)            clusterMean += trainProbs.at<double>(sampleIndex, clusterIndex) * trainSamples.row(sampleIndex);        clusterMean /= weights.at<double>(clusterIndex);    }    // Update covsEigenValues and invCovsEigenValues    covs.resize(nclusters);    covsEigenValues.resize(nclusters);    if(covMatType == EM::COV_MAT_GENERIC)        covsRotateMats.resize(nclusters);    invCovsEigenValues.resize(nclusters);    for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)    {        if(weights.at<double>(clusterIndex) <= minPosWeight)            continue;        if(covMatType != EM::COV_MAT_SPHERICAL)            covsEigenValues[clusterIndex].create(1, dim, CV_64FC1);        else            covsEigenValues[clusterIndex].create(1, 1, CV_64FC1);        if(covMatType == EM::COV_MAT_GENERIC)            covs[clusterIndex].create(dim, dim, CV_64FC1);        Mat clusterCov = covMatType != EM::COV_MAT_GENERIC ?            covsEigenValues[clusterIndex] : covs[clusterIndex];        clusterCov = Scalar(0);        Mat centeredSample;        for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++)        {            centeredSample = trainSamples.row(sampleIndex) - means.row(clusterIndex);            if(covMatType == EM::COV_MAT_GENERIC)                clusterCov += trainProbs.at<double>(sampleIndex, clusterIndex) * centeredSample.t() * centeredSample;            else            {                double p = trainProbs.at<double>(sampleIndex, clusterIndex);                for(int di = 0; di < dim; di++ )                {                    double val = centeredSample.at<double>(di);                    clusterCov.at<double>(covMatType != EM::COV_MAT_SPHERICAL ? di : 0) += p*val*val;                }            }        }        if(covMatType == EM::COV_MAT_SPHERICAL)            clusterCov /= dim;        clusterCov /= weights.at<double>(clusterIndex);        // Update covsRotateMats for EM::COV_MAT_GENERIC only        if(covMatType == EM::COV_MAT_GENERIC)        {            SVD svd(covs[clusterIndex], SVD::MODIFY_A + SVD::FULL_UV);            covsEigenValues[clusterIndex] = svd.w;            covsRotateMats[clusterIndex] = svd.u;        }        max(covsEigenValues[clusterIndex], minEigenValue, covsEigenValues[clusterIndex]);        // update invCovsEigenValues        invCovsEigenValues[clusterIndex] = 1./covsEigenValues[clusterIndex];    }    for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)    {        if(weights.at<double>(clusterIndex) <= minPosWeight)        {            Mat clusterMean = means.row(clusterIndex);            means.row(minWeightClusterIndex).copyTo(clusterMean);//.........这里部分代码省略.........