VecN GeodesicInHeat::compute_heat(const VecN& delta,const Sparse& LapMatrix,const Sparse& areamatrix, float t)
{
	Sparse A = areamatrix - t*LapMatrix;
	Eigen::SimplicialCholesky<Sparse, Eigen::RowMajor> solver;
	solver.compute(A);
	VecN u = solver.solve(delta);
	return u;
}
Exemplo n.º 2
0
    bool solveLinearSystem()
    {
        X.resize (L.rows (), B.cols ());

        // Nothing to solve
        if (L.rows () == 0 || B.cols () == 0)
            return true;

        Eigen::SimplicialCholesky<SparseMatrix, Eigen::Lower> cg;
        cg.compute (L);
        bool succeeded = true;
        for (int i = 0; i < B.cols (); ++i)
        {
            Vector b = B.col (i);
            X.col (i) = cg.solve (b);
            if (cg.info () != Eigen::Success)
                succeeded = false;
        }

        assignColors ();
        return succeeded;
    }
void GeodesicInHeat::compute_distance(const Vertex& start_vertex)
{
	int n = mesh.n_vertices();
	int start_idx = start_vertex.idx();
	VecN delta(n);
	delta.setZero();
	delta(start_idx) =1.0f;

	//The laplacian, gradient divergence matrix only need to compute once.
	if (!initialized)
		initialization();
	float timestep = avglength*avglength*factor;
	VecN uNeumann = compute_heat(delta,Laplacian, AreaMatrix,timestep);
	VecN uDirichlet = compute_heat(delta, LaplacianDirichlet, AreaMatrix, timestep);
	VecN u =0.5*uNeumann+0.5*uDirichlet;

	//Method::the three boundary function will active one, if all false, display the average.
	if (Neumann)
		u = uNeumann;
	if (Dirichlet)
		u = uDirichlet;
	MatMxN X = compute_gradient(Grad_x, Grad_y, Grad_z, u);
	VecN div_X = compute_divergence(Div_x, Div_y, Div_z, X);

	Eigen::SimplicialCholesky<Sparse, Eigen::RowMajor> Solver;
	Solver.compute(Laplacian);
	VecN phi = Solver.solve(div_X);	
	
	//shift phi and find out the Max G, Min G and the Max geodesic distance span along edge.
	for (auto const& vertex : mesh.vertices())
	{
		float d1 = phi[vertex.idx()];
		HeatDisplay[vertex] = d1;
		float d2 = 0.0;
		for (auto const& vj : mesh.vertices(vertex))
		{
			float d = std::abs(phi[vertex.idx()] - phi[vj.idx()]);
			if (d > d2)
				d2 = d;
		}
		if (d1 > MaxGeodesic)
			MaxGeodesic = d1;
		if (d2 > MaxEdgeSpan)
			MaxEdgeSpan = d2;
		if (d1 < MinGeodesic)
			MinGeodesic = d1;
	}
	
	for (auto const& vertex : mesh.vertices())
	{
		HeatDisplay[vertex] = HeatDisplay[vertex] - MinGeodesic;
		HeatDistance[vertex] = HeatDisplay[vertex];
	}
	MaxGeodesic -= MinGeodesic;
	MinGeodesic = 0;

	if (Neumann) {
		mLogger() << "Heat: Neumann Boudary Condition, M=" << factor;
		return;
	}
	if (Dirichlet) {
		mLogger() << "Heat: Dirichlet Boudary Condition, M=" << factor;
		return;
	}
	mLogger() << "Heat: Average Boudary Condition, M=" << factor;
}
static void 
_solve_linear_system(TriMesh& src, const TriMesh& dst, 
    const std::vector<std::vector<int>>& tri_neighbours, 
    const std::vector<std::pair<int, int> >& corres,
    double weights[3]
) {
    int rows = 0; int cols = src.vert_num + src.poly_num - corres.size();
    assert (tri_neighbours.size() == src.poly_num);
    std::vector<std::pair<int, int>> soft_corres;
    _setup_kd_correspondence(src, dst, soft_corres);
#if 0
	std::ofstream ofs("E:/data/ScapeOriginal/build/data_ming/dt_pairs.txt");
	ofs << soft_corres.size() << endl;
	for (size_t i=0; i<soft_corres.size(); ++i)
		ofs << soft_corres[i].first << "\t" << soft_corres[i].second << std::endl;
	printf("check soft pair\n");
	getchar();
#endif
	for (int i=0; i<src.poly_num; ++i)
        rows += 3*tri_neighbours[i].size();        //smooth part
    rows += src.poly_num*3;        //identity part

    static std::vector<bool> vertex_flag; //indicate whether the vertex is hard constrainted by corres
    if (vertex_flag.empty()) {
        vertex_flag.resize(src.vert_num, false);
        for (size_t i=0; i<corres.size(); ++i)
            vertex_flag[corres[i].first] = true;
    }
    
    for (int i=0; i<soft_corres.size(); ++i) {  //soft constraints part
        if (vertex_flag[soft_corres[i].first]) continue;
        ++rows;
	}

    //vertex_real_col stores two information : unknow vertex's col(offset by
    //poly_num) in X and know vertexs' corresponding index in dst mesh.
    //there is no need to compute this in each iteration
    static std::vector<int> vertex_real_col;
    if (vertex_real_col.empty()) {
        vertex_real_col.resize(src.vert_num, -1);
        std::map<int, int> corres_map;
        for (int i=0; i<corres.size(); ++i) corres_map.insert(std::make_pair(corres[i].first, corres[i].second));
        int real_col = 0;
        for (int i=0; i<src.vert_num; ++i) {
            if (vertex_flag[i]) {
                vertex_real_col[i] = corres_map[i];
            } else {
                vertex_real_col[i] = real_col;
                real_col++;
            }
        }
        assert (real_col == src.vert_num - corres.size());  //make sure indexes in corres are different from each other
    }

    SparseMatrix<double> A(rows, cols);  
    A.reserve(Eigen::VectorXi::Constant(cols, 200));
    std::vector<VectorXd> Y(3, VectorXd(rows));
    Y[0].setZero();Y[1].setZero();Y[2].setZero();

    //precompute Q_hat^-1 in Q_hat_inverse   [n v2-v1 v3-v1]^-1
    src.updateNorm();
    assert (!src.face_norm.empty());
    std::vector<Matrix3d> Q_hat_inverse(src.poly_num);
    Eigen::Matrix3d _inverse;
    for (int i=0; i<src.poly_num; ++i) {
        unsigned int* index_i = src.polyIndex[i].vert_index;
        Vector3d v[3];
        v[0] = src.face_norm[i];
        v[1] = src.vertex_coord[index_i[1]] - src.vertex_coord[index_i[0]];//v2-v1
        v[2] = src.vertex_coord[index_i[2]] - src.vertex_coord[index_i[0]];//v3-v1
        for (int k=0; k<3; ++k)
            for (int j=0; j<3; ++j)
                Q_hat_inverse[i](k, j) = v[j][k];
        _inverse = Q_hat_inverse[i].inverse();
        Q_hat_inverse[i] = _inverse;
    }

    int energy_size[3] = {0, 0, 0}; //each energy part's starting index

    //start establishing the large linear sparse system
    double weight_smooth = weights[0]; 
    int row = 0;
    for (int i=0; i<src.poly_num; ++i) {
        Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
        unsigned int* index_i = src.polyIndex[i].vert_index;
        for (size_t _j=0; _j<tri_neighbours[i].size(); ++_j) {
            int j = tri_neighbours[i][_j];    //triangle index
            Eigen::Matrix3d& Q_j_hat = Q_hat_inverse[j];
            unsigned int* index_j = src.polyIndex[j].vert_index;
            for (int k=0; k<3; ++k)
                for (int dim = 0; dim < 3; ++dim)
                    Y[dim](row+k) = 0.0;

            for (int k=0; k<3; ++k) {
                A.coeffRef(row+k, i) = weight_smooth*Q_i_hat(0, k);   //n
                A.coeffRef(row+k, j) = -weight_smooth*Q_j_hat(0, k);   //n
            }

            for (int k=0; k<3; ++k)
                for (int p=0; p<3; ++p)
                    if (!vertex_flag[index_i[p]])
                        A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[p]]) = 0.0;

            if (vertex_flag[index_i[0]]) {
                for (int k=0; k<3; ++k) {
                    for (int dim = 0; dim < 3; ++dim)
                        Y[dim](row + k) += weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
                }
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[0]]) += 
                        -weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k));
            }

            if (vertex_flag[index_j[0]]) {
                for (int k=0; k<3; ++k) {
                    for (int dim = 0; dim < 3; ++dim)
                        Y[dim](row + k) += -weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k))*dst.vertex_coord[vertex_real_col[index_j[0]]][dim];
                }
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[0]]) += 
                        weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k));
            }

            for (int p=1; p<3; ++p) {//v2 or v3
                if (vertex_flag[index_i[p]]) {
                    for (int k=0; k<3; ++k)
                        for (int dim=0; dim<3; ++dim)
                            Y[dim](row + k) += -weight_smooth*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
                } else {
                    for (int k=0; k<3; ++k) 
                        A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) += weight_smooth*Q_i_hat(p, k);
                }
            }

            for (int p=1; p<3; ++p) {
                if (vertex_flag[index_j[p]]) {
                    for (int k=0; k<3; ++k) 
                        for (int dim=0; dim < 3; ++dim)
                            Y[dim](row + k) +=  weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
                } else {
                    for (int k=0; k<3; ++k) 
                        A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
                }
            }
            row += 3;
        }
    }
    energy_size[0] = row;

    double weight_identity = weights[1];
    for (int i=0; i<src.poly_num; ++i) {
        Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
        unsigned int* index_i = src.polyIndex[i].vert_index;
        Y[0](row) = weight_identity;    Y[0](row+1) = 0.0;              Y[0](row+2) = 0.0; 
        Y[1](row) = 0.0;                Y[1](row+1) = weight_identity;  Y[1](row+2) = 0.0; 
        Y[2](row) = 0.0;                Y[2](row+1) = 0.0;              Y[2](row+2) = weight_identity; 

        for (int k=0; k<3; ++k)    
            A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k);   //n

        if (vertex_flag[index_i[0]]) {
            for (int k=0; k<3; ++k) 
                for (int dim = 0; dim < 3; ++dim)
                    Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
        } else {
            for (int k=0; k<3; ++k) 
                A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
        }

        for (int p=1; p<3; ++p) {
            if (vertex_flag[index_i[p]]) {
                for (int k=0; k<3; ++k)
                    for (int dim=0; dim<3; ++dim)
                        Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
            }
        }

        row += 3;
    }
    energy_size[1] = row;

    double weight_soft_constraint = weights[2];
    for (int i=0; i<soft_corres.size(); ++i) {
        if (vertex_flag[soft_corres[i].first]) continue;
        A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
        for (int dim=0; dim<3; ++dim)
            Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
        ++row;
    }
    energy_size[2] = row;

    assert (row == rows);
    
       //start solving the least-square problem
    fprintf(stdout, "finished filling matrix\n");
    Eigen::SparseMatrix<double> At = A.transpose();
    Eigen::SparseMatrix<double> AtA = At*A;

    Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
    solver.compute(AtA);
    if (solver.info() != Eigen::Success) {
        fprintf(stdout, "unable to defactorize AtA\n");
        exit(-1);
    }

	VectorXd X[3];
	for (int i=0; i<3; ++i) {
		VectorXd AtY = At*Y[i];
		X[i] = solver.solve(AtY);
		Eigen::VectorXd Energy = A*X[i] - Y[i];
		Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
		Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
		Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
		fprintf(stdout, "\t%lf = %lf + %lf + %lf\n", 
			Energy.dot(Energy), smoothEnergy.dot(smoothEnergy), 
			identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
	}
    
    //fill data back to src
    for (int i=0; i<src.poly_num; ++i)
        for (int d=0; d<3; ++d)
            src.face_norm[i][d] = X[d](i);
    for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
    int p = 0;
    for (int i=0; i<src.vert_num; ++i) {
        if (vertex_flag[i]) continue;
        for (int d=0; d<3; ++d)
            src.vertex_coord[i][d] = X[d](src.poly_num+p);
        ++p;
    }
    return;
}
void MsInterpolation::build_interpolation( FaceNode *subroot, MyMesh &src_mesh, MyMesh &target_mesh, double t )
{
	bool leaf_node = true;
 
	for (int i = 0; i != NR_MIN_PATCH_PER_LEVEL; i++)
	{
		if( subroot->next[i] != NULL)
		{
			leaf_node = false;
			break;
		}
	}

	/***如果是叶节点***/
	if( leaf_node )
	{
		leaf_node_interpolation(t, subroot, src_mesh,target_mesh);
		return;
	}

	for (int i = 0; i != NR_MIN_PATCH_PER_LEVEL; i++)
	{
		build_interpolation(subroot->next[i], src_mesh, target_mesh, t);
	}

	/***进行配准***/
	int M = NR_MIN_PATCH_PER_LEVEL;
	int P = subroot->P.size();

	std::vector<PointList> pl;

	for (int i = 0; i != P; i++)
	{
		int a = subroot->P[i].a;
		int b = subroot->P[i].b;

		FaceNode *node_a = subroot->next[a];
		FaceNode *node_b = subroot->next[b];
		
		std::vector<int> *curr_p = &subroot->pl[i];
		int size = curr_p->size();
		
		PointList pts;

		for (int j = 0; j != size; j++)
		{
			int idx = (*curr_p)[j];//共有的点索引
			PairVertex pv;

			pv.a = node_a->pts[ node_a->r_idx[idx] ];
			pv.b = node_b->pts[ node_b->r_idx[idx] ];


			pts.push_back( pv );
		}

		pl.push_back( pts );
	}

	MultiRegistration mr(M, P, &subroot->P, &pl);
	mr.init();

	std::vector<MyMatrix3f> R;
	std::vector<MyVector3f> T;

	mr.get_R_and_T(R, T);

	/***转换到统一的坐标系中***/
	for (int i = 0; i != NR_MIN_PATCH_PER_LEVEL; i++)
	{
		int size = subroot->next[i]->pts.size();

		for (int j = 0; j != size; j++)
		{
			subroot->next[i]->pts[j] = R[i] * subroot->next[i]->pts[j] + T[i];
		}

	}

	/***分配空间***/
	int size = subroot->pts_index.size();
	subroot->pts.assign(size, MyVector3f(0, 0, 0) );

	if( BLENDING )
	{
		int m = 0;
		MyMatrixXf E;

		E.resize(subroot->M.rows(), 3);

		for (int i = 0; i != NR_MIN_PATCH_PER_LEVEL; i++)
		{
			FaceNode *node_i = subroot->next[i];
			int nr_edges = subroot->edges_vector[i].size();

			for (int j = 0; j !=  nr_edges; j++)
			{
				int g_from = subroot->edges_vector[i][j].a;
				int g_to = subroot->edges_vector[i][j].b;

				int n_from =node_i->r_idx[ g_from ];
				int n_to = node_i->r_idx[ g_to ];

				MyVector3f e = node_i->pts[n_from] - node_i->pts[n_to];

				int from = subroot->r_idx[ g_from ];
				int to = subroot->r_idx[ g_to ];

				assert( subroot->M(m, from) == 1);
				assert( subroot->M(m, to) == -1);

				E(m, 0) = e(0);
				E(m, 1) = e(1);
				E(m, 2) = e(2);
				m++;
			}
		}

		E(m, 0) = 0;
		E(m, 1) = 0;
		E(m, 2) = 0;

		Eigen::SimplicialCholesky<MySMatrixXf > solver;
		solver.compute( (subroot->M.transpose() * subroot->M) );

		MyMatrixXf result = solver.solve( subroot->M.transpose() * E );

		int pts_size = subroot->pts_index.size();

		for (int i = 0; i != pts_size; i++)
		{
			double x, y, z;

			if( fabs( result(i, 0) ) < 1e-10 )
			{
				x = 0;
			}else
			{
				x = result(i, 0);
			}

			if( fabs( result(i, 1) ) < 1e-10 )
			{
				y = 0;
			}else
			{
				y = result(i, 1);
			}

			if( fabs( result(i, 2) ) < 1e-10 )
			{
				z = 0;
			}else
			{
				z = result(i, 2);
			}

			MyVector3f vi = MyVector3f(x, y, z);
			subroot->pts[i] = vi;
		}

	}else
	{
		// 如果没有blending的过程,则平均
		std::vector<int> cnt;

		cnt.assign( size, 0);

		for (int i = 0; i != NR_MIN_PATCH_PER_LEVEL; i++)
		{
			FaceNode *node_i = subroot->next[i];
			int pts_size = node_i->pts_index.size();

			for (int j = 0; j != pts_size; j++)
			{
				int idx = node_i->pts_index[j];
				int r_idx = subroot->r_idx[ idx ];

				cnt[r_idx] += 1;

				subroot->pts[ r_idx ] += node_i->pts[j];
			}

		}

		/**** 对于重复的点的坐标取平均值 ***/
		for (int i = 0; i != size; i++)
		{

			subroot->pts[ i ]  /= cnt[i];

		}
	}
}