1 void mainNode::InitLpls()
 2 {
 3     int vertexNumber = m_vecAllVertex.size();
 4     //计算拉普拉斯坐标。方式一
 5     for (int i = 0; i < vertexNumber; i++)
 6     {
 7         pVERTEX pVertex = m_vecAllVertex.at(i);
 8         osg::Vec3 neighborPos(0.0, 0.0, 0.0);
 9         for (int j = 0; j < pVertex->vecNeighborVertex.size(); j++)
10         {
11             neighborPos += pVertex->vecNeighborVertex.at(j)->pos;
12         }
13         pVertex->lplsPos = pVertex->pos - neighborPos / pVertex->vecNeighborVertex.size();
14     }
15 
16     //计算顶点一阶临接顶点的权值
17     for (int i = 0; i < vertexNumber; i++)
18     {
19         pVERTEX pVertex = m_vecAllVertex.at(i);
20         int neighborNumber = pVertex->vecNeighborVertex.size();
21         for (int j = 0; j < neighborNumber; j++)
22         {
23             pVERTEX pNeighbor = pVertex->vecNeighborVertex.at(j);
24             float weight = float(1) / float(neighborNumber);
25             pVertex->mapWeightForOther.insert(std::pair<int, float>(pNeighbor->id, weight) );
26             pVertex->fTotalWeight = 1.0;
27         }
28     }
29 
30     //构建拉普拉斯矩阵
31     int count = 0;
32     std::vector<int> beginNumber(vertexNumber);
33     for (int i = 0; i < vertexNumber; i++)
34     {
35         beginNumber[i] = count;
36         count += m_vecAllVertex[i]->vecNeighborVertex.size() + 1;
37     }
38 
39     std::vector<Eigen::Triplet<float> > vectorTriplet(count);
40     for (int i = 0; i < vertexNumber; i++)
41     {
42         pVERTEX pVertex = m_vecAllVertex.at(i);
43         vectorTriplet[beginNumber[i]] = Eigen::Triplet<float>(i, i, pVertex->fTotalWeight);
44 
45         int j = 0;
46         std::map<int, float>::iterator itor;
47         for(itor = pVertex->mapWeightForOther.begin(); itor != pVertex->mapWeightForOther.end(); itor++)
48         {
49             int neighborID = itor->first;
50             float weight = itor->second;
51             vectorTriplet[beginNumber[i] + j + 1] = Eigen::Triplet<float>(i, neighborID, -weight);
52             j++;
53         }
54     }
55 
56     //加入移动点以及不动点，这里便是调参的地方
57     for (int i = 0; i < 2; i++)
58     {
59         pVERTEX pVertex;
60         if (i == 0)
61         {
62             pVertex = m_vecAllVertex.at(i);
63         }
64         else
65         {
66             pVertex = m_vecAllVertex.at(vertexNumber - 1);
67         }
68 
69         int row = i + vertexNumber;
70         vectorTriplet.push_back(Eigen::Triplet<float>(row, pVertex->id, pVertex->fTotalWeight));
71     }
72     
73     m_vectorTriplet = vectorTriplet;
74     m_Matrix.resize(vertexNumber + 2, vertexNumber);
75     m_Matrix.setFromTriplets(vectorTriplet.begin(), vectorTriplet.end());
76 }

 1 void mainNode::CalculateMaitrxLpls()
 2 {
 3     Eigen::SparseMatrix<float> Matrix = m_Matrix;
 4     Eigen::SparseMatrix<float> MatrixTranspose = Matrix.transpose();
 5     Eigen::SparseMatrix<float> MatrixLpls = MatrixTranspose * Matrix;
 6 
 7     Eigen::VectorXf targetX, targetY, targetZ;
 8     int vertexNumber = m_vecAllVertex.size();
 9     
10     targetX.resize(vertexNumber + 2);
11     targetY.resize(vertexNumber + 2);
12     targetZ.resize(vertexNumber + 2);
13     
14     for (int i = 0; i < vertexNumber + 2; i++)
15     {
16         if (i < vertexNumber)
17         {
18             targetX[i] = m_vecAllVertex.at(i)->lplsPos.x();
19             targetY[i] = m_vecAllVertex.at(i)->lplsPos.y();
20             targetZ[i] = m_vecAllVertex.at(i)->lplsPos.z();
21         }
22         else if (i < vertexNumber + 1)
23         {
24             //第一个的顶点坐标分量
25             targetX[i] = m_vecAllVertex.at(i - vertexNumber)->pos.x();
26             targetY[i] = m_vecAllVertex.at(i - vertexNumber)->pos.y();
27             targetZ[i] = m_vecAllVertex.at(i - vertexNumber)->pos.z();
28         }
29         else
30         {
31             //最后一个的顶点坐标分量
32             targetX[i] = m_vecAllVertex.at(vertexNumber - 1)->pos.x();
33             targetY[i] = m_vecAllVertex.at(vertexNumber - 1)->pos.y();
34             targetZ[i] = m_vecAllVertex.at(vertexNumber - 1)->pos.z();
35         }
36     }
37     
38     Eigen::SimplicialCholesky<Eigen::SparseMatrix<float> > MatrixCholesky(MatrixLpls);
39     Eigen::VectorXf resX, resY, resZ;
40     resX = MatrixTranspose * targetX;
41     resY = MatrixTranspose * targetY;
42     resZ = MatrixTranspose * targetZ;
43 
44     Eigen::VectorXf X, Y, Z;
45     X = MatrixCholesky.solve(resX);
46     //std::cout << X << std::endl;
47     Y = MatrixCholesky.solve(resY);
48     //std::cout << Y << std::endl;
49     Z = MatrixCholesky.solve(resZ);
50     //std::cout << Z << std::endl;
51     for (int i = 0; i < m_vecAllVertex.size(); i++)
52     {
53         pVERTEX pVertex = m_vecAllVertex.at(i);
54         float x, y, z;
55         x = X[i];
56         y = Y[i];
57         z = Z[i];
58 
59         pVertex->pos = osg::Vec3(X[i], Y[i], Z[i]);
60     }
61 }