CameraPoseOptimization::~CameraPoseOptimization()
{
m_frameInfos.clear();
FreeVectorMemory(m_frameInfos);
for (int i = 0; i != m_keypointNodesInFrames.size(); ++i)
{
m_keypointNodesInFrames[i].clear();
FreeVectorMemory(m_keypointNodesInFrames[i]);
}
m_keypointNodesInFrames.clear();
FreeVectorMemory(m_keypointNodesInFrames);
for (int i = 0; i != m_rowCorrespondenceList.size(); ++i)
{
for (int j = 0; j != m_rowCorrespondenceList[i].size(); ++j)
{
m_rowCorrespondenceList[i][j] = 0;
}
m_rowCorrespondenceList[i].clear();
}
m_rowCorrespondenceList.clear();
FreeVectorMemory(m_rowCorrespondenceList);
for (int i = 0; i != m_columnCorrespondenceList.size(); ++i)
{
for (int j = 0; j != m_columnCorrespondenceList[i].size(); ++j)
{
m_columnCorrespondenceList[i][j] = 0;
}
m_columnCorrespondenceList[i].clear();
}
m_columnCorrespondenceList.clear();
FreeVectorMemory(m_columnCorrespondenceList);
}
void CalcCombinations ( rvector & combs, int & vlen,
PCSSGraph G1, PCSSGraph G2 ) {
// combs[i], i=1..vlen, returns the number of common
// substructures of size i of graphs G1 and G2. The
// sequential order of graph vertices is taken into
// account, however the actual edges are completely
// neglected.
PPCSSVertex V1,V2;
rmatrix3 P;
imatrix C;
realtype q;
int n,m, i,j,k;
n = G1->GetNofVertices();
m = G2->GetNofVertices();
if (n<=m) {
V1 = G1->GetVertices();
V2 = G2->GetVertices();
} else {
m = G1->GetNofVertices();
n = G2->GetNofVertices();
V2 = G1->GetVertices();
V1 = G2->GetVertices();
}
vlen = 0;
FreeVectorMemory ( combs,1 );
if (n<=0) return;
GetMatrix3Memory ( P,n,m,n,1,1,1 );
GetMatrixMemory ( C,n,m,1,1 );
for (i=1;i<=n;i++)
for (j=1;j<=m;j++) {
if (V1[i-1]->Compare(V2[j-1])) C[i][j] = 1;
else C[i][j] = 0;
for (k=1;k<=n;k++)
P[i][j][k] = 0.0;
}
q = 0.0;
for (j=1;j<=m;j++) {
q += C[1][j];
P[1][j][1] = q;
}
for (i=2;i<=n;i++) {
q = 0.0;
for (j=1;j<=m;j++) {
q += C[i][j];
P[i][j][1] = P[i-1][j][1] + q;
}
for (k=2;k<=i;k++) {
for (j=k;j<=m;j++)
if (C[i][j]==0) P[i][j][k] = P[i][j-1][k];
else P[i][j][k] = P[i][j-1][k] + P[i-1][j-1][k-1];
for (j=k;j<=m;j++)
P[i][j][k] += P[i-1][j][k];
}
}
vlen = n;
GetVectorMemory ( combs,n,1 );
for (k=1;k<=n;k++)
combs[k] = P[n][m][k];
FreeMatrix3Memory ( P,n,m,1,1,1 );
FreeMatrixMemory ( C,n,1,1 );
}
int SuperposeSSGraphs ( PCSSGraph G1, ivector F1,
PCSSGraph G2, ivector F2,
int matchlen,
mat44 & TMatrix ) {
PCSSVertex Vx;
rmatrix A,U,V;
rvector W,RV1;
vect3 v1,v2;
realtype det,B, x01,y01,z01, x02,y02,z02, mass,mass1,mass2;
int i,j,k,l, nE1,nE2;
nE1 = G1->GetNofEdges();
if (!nE1) G1->BuildGraph();
nE2 = G2->GetNofEdges();
if (!nE2) G2->BuildGraph();
GetMatrixMemory ( A,3,3,1,1 );
GetMatrixMemory ( U,3,3,1,1 );
GetMatrixMemory ( V,3,3,1,1 );
GetVectorMemory ( W,3,1 );
GetVectorMemory ( RV1,3,1 );
for (j=1;j<=3;j++)
for (k=1;k<=3;k++)
A[j][k] = 0.0;
for (i=1;i<=matchlen;i++) {
Vx = G1->GetGraphVertex ( F1[i] );
Vx->GetDirection ( v1 );
Vx = G2->GetGraphVertex ( F2[i] );
Vx->GetDirection ( v2 );
for (j=1;j<=3;j++)
for (k=1;k<=3;k++)
A[j][k] += v1[k-1]*v2[j-1];
}
for (i=1;i<matchlen;i++)
for (l=i+1;l<=matchlen;l++)
if (G1->GetEdgeDirection(F1[i],F1[l],v1) &&
G2->GetEdgeDirection(F2[i],F2[l],v2))
for (j=1;j<=3;j++)
for (k=1;k<=3;k++)
A[j][k] += v1[k-1]*v2[j-1];
det = A[1][1]*A[2][2]*A[3][3] +
A[1][2]*A[2][3]*A[3][1] +
A[2][1]*A[3][2]*A[1][3] -
A[1][3]*A[2][2]*A[3][1] -
A[1][1]*A[2][3]*A[3][2] -
A[3][3]*A[1][2]*A[2][1];
SVD ( 3,3,3,A,U,V,W,RV1,True,True,i );
if (i!=0) {
for (j=0;j<4;j++) {
for (k=0;k<4;k++)
TMatrix[j][k] = 0.0;
TMatrix[j][j] = 1.0;
}
return 1;
}
if (det<0.0) {
k = 0;
B = MaxReal;
for (j=1;j<=3;j++)
if (W[j]<B) {
B = W[j];
k = j;
}
for (j=1;j<=3;j++)
V[k][j] = -V[k][j];
}
for (j=1;j<=3;j++)
for (k=1;k<=3;k++) {
B = 0.0;
for (i=1;i<=3;i++)
B += U[j][i]*V[k][i];
TMatrix[j-1][k-1] = B;
}
// 9. Add translation
x01 = 0.0; y01 = 0.0; z01 = 0.0; mass1 = 0.0;
x02 = 0.0; y02 = 0.0; z02 = 0.0; mass2 = 0.0;
for (i=1;i<=matchlen;i++) {
Vx = G1->GetGraphVertex ( F1[i] );
mass = Vx->GetMass();
Vx->GetPosition ( v1 );
x01 += v1[0]*mass;
y01 += v1[1]*mass;
z01 += v1[2]*mass;
mass1 += mass;
Vx = G2->GetGraphVertex ( F2[i] );
mass = Vx->GetMass();
Vx->GetPosition ( v2 );
x02 += v2[0]*mass;
y02 += v2[1]*mass;
z02 += v2[2]*mass;
mass2 += mass;
}
x01 /= mass1; y01 /= mass1; z01 /= mass1;
x02 /= mass2; y02 /= mass2; z02 /= mass2;
TMatrix[0][3] = x02 - TMatrix[0][0]*x01 - TMatrix[0][1]*y01 -
TMatrix[0][2]*z01;
TMatrix[1][3] = y02 - TMatrix[1][0]*x01 - TMatrix[1][1]*y01 -
TMatrix[1][2]*z01;
TMatrix[2][3] = z02 - TMatrix[2][0]*x01 - TMatrix[2][1]*y01 -
TMatrix[2][2]*z01;
FreeMatrixMemory ( A,1,1 );
FreeMatrixMemory ( U,1,1 );
FreeMatrixMemory ( V,1,1 );
FreeVectorMemory ( W,1 );
FreeVectorMemory ( RV1,1 );
if (!nE1) G1->ReleaseEdges();
if (!nE2) G2->ReleaseEdges();
return 0;
}
void Mesh::Construct(ID3D11Device* d3dDevice)
{
if (pos.size() == 0)
{
return;
}
// validate sizes are the same
if (!SizeCheck(nor.size(), pos.size(), "nor", "pos") || !SizeCheck(tex.size(), pos.size(), "tex", "pos"))
{
return;
}
// if tangents exist, then validate they are the same size as pos
if (!SizeCheck(tan.size(), pos.size(), "tan", "pos"))
{
return;
}
SAFE_DELETE(vertexBuffer);
SAFE_DELETE(indexBuffer);
// create index buffer
if(indices.size() > 0)
{
indexBuffer = GpuResourceFactory::CreateIndexBuffer(&indices[0], (uint32_t)indices.size() );
indexBuffer->SetDebugName(name.c_str());
}
if(tan.size() > 0)
{
std::vector<VertexPNTT> verts;
verts.reserve(pos.size());
for(unsigned int i = 0; i < pos.size(); ++i)
{
VertexPNTT vert;
vert.Position = pos[i];
vert.Normal = nor[i];
vert.Tangent = tan[i];
vert.Tex = tex[i];
verts.push_back(vert);
}
vertexBuffer = GpuResourceFactory::CreateVertexBuffer(&verts[0], VertexFormat::VF_PNTT, (uint32_t)verts.size());
}
else if (tex.size() > 0)
{
std::vector<VertexPNT> verts;
verts.reserve(pos.size());
for(unsigned int i = 0; i < pos.size(); ++i)
{
VertexPNT vert;
vert.Position = pos[i];
vert.Normal = nor[i];
vert.Tex = tex[i];
verts.push_back(vert);
}
vertexBuffer = GpuResourceFactory::CreateVertexBuffer(&verts[0],VertexFormat::VF_PNT, (uint32_t)verts.size());
}
else if (nor.size() > 0)
{
std::vector<VertexPN> verts;
verts.reserve(pos.size());
for(unsigned int i = 0; i < pos.size(); ++i)
{
VertexPN vert;
vert.Position = pos[i];
vert.Normal = nor[i];
verts.push_back(vert);
}
vertexBuffer = GpuResourceFactory::CreateVertexBuffer(&verts[0],VertexFormat::VF_PN, (uint32_t)verts.size());
}
else
{
vertexBuffer = GpuResourceFactory::CreateVertexBuffer(&pos[0],VertexFormat::VF_P, (uint32_t)pos.size());
}
FreeVectorMemory(tex);
FreeVectorMemory(tan);
}
