void getTriangulateCovMat(int nview, const double* K, const double* R,
const double* t, const double x[3], double cov[9], double sigma) {
double * J = new double[nview * 12];
for (int i = 0; i < nview; i++) {
computePerspectiveJacobian(K + 9 * i, R + 9 * i, t + 3 * i, x,
J + 6 * i);
}
double tmp[9];
matATB(nview * 2, 3, nview * 2, 3, J, J, tmp);
matInv(3, tmp, cov);
sigma *= sigma;
cov[0] *= sigma;
cov[1] *= sigma;
cov[2] *= sigma;
cov[3] *= sigma;
cov[4] *= sigma;
cov[5] *= sigma;
cov[6] *= sigma;
cov[7] *= sigma;
cov[8] *= sigma;
delete[] J;
}
static void intraCamCovWeightedLMStep(const double K[9],
const double R[9],
const double t[3],
int npts,
const double covs[],
const double Ws[],
const double Ms[],
const double ms[],
double param[6],
const double lambda) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pcovs = covs;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pcovs += 9, pms += 2) {
double var[4], ivar[4];
project(K, R, t, pMs, rm);
getProjectionCovMat(K, R, t, pMs, pcovs, var, 1.0);
mat22Inv(var, ivar);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
//J:\in R ^{2x6}
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
//\sum_i w[i]*(J_\omega^T *ivar[i] * J_\omega)
double Jivar[12];
matATB(2, 6, 2, 2, J, ivar, Jivar);
matAB(6, 2, 2, 6, Jivar, J, A);
mat66scale(A, Ws[i]);
mat66sum(sA, A, sA);
//-\sum_i w[i]*( J_\omega^T *ivar[i] *\epsilon_i)
rerr[0] = (-rm[0] + pms[0]);
rerr[1] = (-rm[1] + pms[1]);
matAB(6, 2, 2, 1, Jivar, rerr, B);
mat16scale(B, Ws[i]);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
void seqTriangulate(const double K[9], const double R[9], const double t[3],
const double m[2], double M[3], double cov[3], double sigma) {
double rm[2], dm[2];
project(K, R, t, M, rm);
dm[0] = m[0] - rm[0];
dm[1] = m[1] - rm[1];
//compute Jacobian:
double J[6];
computePerspectiveJacobian(K, R, t, M, J);
//compute 2D covariance:
double JC[6], S[4], iS[4];
//JC = J*cov
//S = JC*J'
// S = J*cov*J'
matAB(2, 3, 3, 3, J, cov, JC);
matABT(2, 3, 2, 3, JC, J, S);
double sigma2 = sigma * sigma;
S[0] += sigma2;
S[3] += sigma2;
//iS = (sigma+S)^-1
matInv(2, S, iS);
//compute Kalman gain:
double CJT[6], G[6];
//CJT = cov*J'
//G = cov*J'*iS = cov*J'*(sigma+J*cov*J')^-1
matABT(3, 3, 2, 3, cov, J, CJT);
matAB(3, 2, 2, 2, CJT, iS, G);
//update 3D position
double dM[3];
matAB(3, 2, 2, 1, G, dm, dM);
M[0] += dM[0];
M[1] += dM[1];
M[2] += dM[2];
//update 3D covariance
//cov - G*J^T*cov
double GJ[9], GJC[9];
matAB(3, 2, 2, 3, G, J, GJ);
mat33AB(GJ, cov, GJC);
mat33Diff(cov, GJC, cov);
}
static void intraCamWeightedLMStep(const double K[9],
const double R[9],
const double t[3],
int npts,
const double Ws[],
const double Ms[],
const double ms[],
double param[6],
const double lambda) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Ws[i] * Jw[0], Ws[i] * Jw[1], Ws[i] * Jw[2], Ws[i] * Jt[0], Ws[i] * Jt[1], Ws[i] * Jt[2],
Ws[i] * Jw[3], Ws[i] * Jw[4], Ws[i] * Jw[5], Ws[i] * Jt[3], Ws[i] * Jt[4], Ws[i] * Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = (-rm[0] + pms[0]) * Ws[i];
rerr[1] = (-rm[1] + pms[1]) * Ws[i];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
/**
* compute a new parameter for each iterative step
*/
static void intraCamLMStep(const double K[9], const double R[9], //current parameter
const double t[3],
int npts,
const double Ms[], //corresponding 3D-2D points
const double ms[],
double param[6], //new parameter
const double lambda //damping coefficient
) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2];
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < npts; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = -rm[0] + pms[0];
rerr[1] = -rm[1] + pms[1];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
void getBinTriangulateCovMat(const double* K1, const double* R1,
const double* t1, const double* K2, const double* R2, const double* t2,
double x[3], double cov[9], double sigma) {
double J[12];
computePerspectiveJacobian(K1, R1, t1, x, J);
computePerspectiveJacobian(K2, R2, t2, x, J + 6);
double tmp[9];
matATB(4, 3, 4, 3, J, J, tmp);
matInv(3, tmp, cov);
sigma *= sigma;
cov[0] *= sigma;
cov[1] *= sigma;
cov[2] *= sigma;
cov[3] *= sigma;
cov[4] *= sigma;
cov[5] *= sigma;
cov[6] *= sigma;
cov[7] *= sigma;
cov[8] *= sigma;
}
dgMatrix dgMatrix::Symetric3by3Inverse () const
{
const dgMatrix& mat = *this;
hacd::HaF64 det = mat[0][0] * mat[1][1] * mat[2][2] +
mat[0][1] * mat[1][2] * mat[0][2] * hacd::HaF32 (2.0f) -
mat[0][2] * mat[1][1] * mat[0][2] -
mat[0][1] * mat[0][1] * mat[2][2] -
mat[0][0] * mat[1][2] * mat[1][2];
det = hacd::HaF32 (1.0f) / det;
hacd::HaF32 x11 = (hacd::HaF32)(det * (mat[1][1] * mat[2][2] - mat[1][2] * mat[1][2]));
hacd::HaF32 x22 = (hacd::HaF32)(det * (mat[0][0] * mat[2][2] - mat[0][2] * mat[0][2]));
hacd::HaF32 x33 = (hacd::HaF32)(det * (mat[0][0] * mat[1][1] - mat[0][1] * mat[0][1]));
hacd::HaF32 x12 = (hacd::HaF32)(det * (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]));
hacd::HaF32 x13 = (hacd::HaF32)(det * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));
hacd::HaF32 x23 = (hacd::HaF32)(det * (mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]));
#ifdef _DEBUG
dgMatrix matInv (dgVector (x11, x12, x13, hacd::HaF32(0.0f)),
dgVector (x12, x22, x23, hacd::HaF32(0.0f)),
dgVector (x13, x23, x33, hacd::HaF32(0.0f)),
dgVector (hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(1.0f)));
dgMatrix test (matInv * mat);
HACD_ASSERT (dgAbsf (test[0][0] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
HACD_ASSERT (dgAbsf (test[1][1] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
HACD_ASSERT (dgAbsf (test[2][2] - hacd::HaF32(1.0f)) < hacd::HaF32(0.01f));
#endif
return dgMatrix (dgVector (x11, x12, x13, hacd::HaF32(0.0f)),
dgVector (x12, x22, x23, hacd::HaF32(0.0f)),
dgVector (x13, x23, x33, hacd::HaF32(0.0f)),
dgVector (hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(0.0f), hacd::HaF32(1.0f)));
}
/*
function code = codegen_DFDu()
a = sym('a'); b = sym('b'); c = sym('c'); d = sym('d');
e = sym('e'); f = sym('f'); g = sym('g'); h = sym('h');
i = sym('i'); j = sym('j'); k = sym('k'); l = sym('l');
m = sym('m'); n = sym('n'); o = sym('o'); p = sym('p');
q = sym('q'); r = sym('r'); s = sym('s'); t = sym('t');
v = sym('u');
n0 = [a b c];
n1 = [d e f];
n2 = [g h i];
n3 = [j k l];
u = mytrans([a b c d e f g h i j k l]);
x = mytrans([
n1-n0
n2-n0
n3-n0
]);
Xinv = [
m n o
p q r
s t v
];
F = x * Xinv;
F = reshape(F, 9, 1);
pF_pu = sym('a') * ones(9, 12);
for i = 1:9
for j = 1:12
pF_pu(i,j) = diff( F(i), u(j) );
end
end
code = maple('codegen[C]', pF_pu, 'optimized');
end
*/
void TET::computePFPu()
{
const MATRIX3& matInv = _DmInv;
const double m = matInv(0,0);
const double n = matInv(0,1);
const double o = matInv(0,2);
const double p = matInv(1,0);
const double q = matInv(1,1);
const double r = matInv(1,2);
const double s = matInv(2,0);
const double t = matInv(2,1);
const double u = matInv(2,2);
const double t1 = -m-p-s;
const double t2 = -n-q-t;
const double t3 = -o-r-u;
_PFPu.resize(9, 12);
_PFPu.setZero();
_PFPu(0,0) = t1;
_PFPu(0,1) = 0.0;
_PFPu(0,2) = 0.0;
_PFPu(0,3) = m;
_PFPu(0,4) = 0.0;
_PFPu(0,5) = 0.0;
_PFPu(0,6) = p;
_PFPu(0,7) = 0.0;
_PFPu(0,8) = 0.0;
_PFPu(0,9) = s;
_PFPu(0,10) = 0.0;
_PFPu(0,11) = 0.0;
_PFPu(1,0) = 0.0;
_PFPu(1,1) = t1;
_PFPu(1,2) = 0.0;
_PFPu(1,3) = 0.0;
_PFPu(1,4) = m;
_PFPu(1,5) = 0.0;
_PFPu(1,6) = 0.0;
_PFPu(1,7) = p;
_PFPu(1,8) = 0.0;
_PFPu(1,9) = 0.0;
_PFPu(1,10) = s;
_PFPu(1,11) = 0.0;
_PFPu(2,0) = 0.0;
_PFPu(2,1) = 0.0;
_PFPu(2,2) = t1;
_PFPu(2,3) = 0.0;
_PFPu(2,4) = 0.0;
_PFPu(2,5) = m;
_PFPu(2,6) = 0.0;
_PFPu(2,7) = 0.0;
_PFPu(2,8) = p;
_PFPu(2,9) = 0.0;
_PFPu(2,10) = 0.0;
_PFPu(2,11) = s;
_PFPu(3,0) = t2;
_PFPu(3,1) = 0.0;
_PFPu(3,2) = 0.0;
_PFPu(3,3) = n;
_PFPu(3,4) = 0.0;
_PFPu(3,5) = 0.0;
_PFPu(3,6) = q;
_PFPu(3,7) = 0.0;
_PFPu(3,8) = 0.0;
_PFPu(3,9) = t;
_PFPu(3,10) = 0.0;
_PFPu(3,11) = 0.0;
_PFPu(4,0) = 0.0;
_PFPu(4,1) = t2;
_PFPu(4,2) = 0.0;
_PFPu(4,3) = 0.0;
_PFPu(4,4) = n;
_PFPu(4,5) = 0.0;
_PFPu(4,6) = 0.0;
_PFPu(4,7) = q;
_PFPu(4,8) = 0.0;
_PFPu(4,9) = 0.0;
_PFPu(4,10) = t;
_PFPu(4,11) = 0.0;
_PFPu(5,0) = 0.0;
_PFPu(5,1) = 0.0;
_PFPu(5,2) = t2;
_PFPu(5,3) = 0.0;
_PFPu(5,4) = 0.0;
_PFPu(5,5) = n;
_PFPu(5,6) = 0.0;
_PFPu(5,7) = 0.0;
_PFPu(5,8) = q;
_PFPu(5,9) = 0.0;
_PFPu(5,10) = 0.0;
_PFPu(5,11) = t;
_PFPu(6,0) = t3;
_PFPu(6,1) = 0.0;
_PFPu(6,2) = 0.0;
_PFPu(6,3) = o;
_PFPu(6,4) = 0.0;
_PFPu(6,5) = 0.0;
_PFPu(6,6) = r;
_PFPu(6,7) = 0.0;
_PFPu(6,8) = 0.0;
_PFPu(6,9) = u;
_PFPu(6,10) = 0.0;
_PFPu(6,11) = 0.0;
_PFPu(7,0) = 0.0;
_PFPu(7,1) = t3;
_PFPu(7,2) = 0.0;
_PFPu(7,3) = 0.0;
_PFPu(7,4) = o;
_PFPu(7,5) = 0.0;
_PFPu(7,6) = 0.0;
_PFPu(7,7) = r;
_PFPu(7,8) = 0.0;
_PFPu(7,9) = 0.0;
_PFPu(7,10) = u;
_PFPu(7,11) = 0.0;
_PFPu(8,0) = 0.0;
_PFPu(8,1) = 0.0;
_PFPu(8,2) = t3;
_PFPu(8,3) = 0.0;
_PFPu(8,4) = 0.0;
_PFPu(8,5) = o;
_PFPu(8,6) = 0.0;
_PFPu(8,7) = 0.0;
_PFPu(8,8) = r;
_PFPu(8,9) = 0.0;
_PFPu(8,10) = 0.0;
_PFPu(8,11) = u;
}
// Create meshes from the 3ds File
void GLC_3dsToWorld::createMeshes(GLC_StructOccurence* pProduct, Lib3dsNode* pFatherNode)
{
GLC_StructOccurence* pChildProduct= NULL;
Lib3dsMesh *pMesh= NULL;
if (pFatherNode->type == LIB3DS_OBJECT_NODE)
{
//qDebug() << "Node type LIB3DS_OBJECT_NODE is named : " << QString(pFatherNode->name);
//qDebug() << "Node Matrix :";
//qDebug() << GLC_Matrix4x4(&(pFatherNode->matrix[0][0])).toString();
// Check if the node is a mesh or dummy
if (!(strcmp(pFatherNode->name,"$$$DUMMY")==0))
{
pMesh = lib3ds_file_mesh_by_name(m_pLib3dsFile, pFatherNode->name);
if( pMesh != NULL )
{
GLC_3DRep representation(create3DRep(pMesh));
// Test if there is vertex in the mesh
if (0 != representation.vertexCount())
{
m_LoadedMeshes.insert(representation.name());
// Load node matrix
GLC_Matrix4x4 nodeMat(&(pFatherNode->matrix[0][0]));
// The mesh matrix to inverse
GLC_Matrix4x4 matInv(&(pMesh->matrix[0][0]));
matInv.invert();
// Get the node pivot
Lib3dsObjectData *pObjectData;
pObjectData= &pFatherNode->data.object;
GLC_Matrix4x4 trans(-pObjectData->pivot[0], -pObjectData->pivot[1], -pObjectData->pivot[2]);
// Compute the part matrix
nodeMat= nodeMat * trans * matInv; // I don't know why...
nodeMat.optimise();
// move the part by the matrix
pProduct->addChild((new GLC_StructInstance(new GLC_3DRep(representation)))->move(nodeMat));
}
else
{
// the instance will be deleted, check material usage
QSet<GLC_Material*> meshMaterials= representation.materialSet();
QSet<GLC_Material*>::const_iterator iMat= meshMaterials.constBegin();
while (iMat != meshMaterials.constEnd())
{
if ((*iMat)->numberOfUsage() == 1)
{
m_Materials.remove((*iMat)->name());
}
++iMat;
}
}
}
} // End If DUMMY
}
else return;
// If there is a child, create a child product
if (NULL != pFatherNode->childs)
{
pChildProduct= new GLC_StructOccurence();
pProduct->addChild(pChildProduct);
pChildProduct->setName(QString("Product") + QString::number(pFatherNode->node_id));
//pChildProduct->move(GLC_Matrix4x4(&(pFatherNode->matrix[0][0])));
// Create Childs meshes if exists
for (Lib3dsNode* pNode= pFatherNode->childs; pNode!=0; pNode= pNode->next)
{
createMeshes(pChildProduct, pNode);
}
}
}
static void intraCamEpiLMStep(const double K[9], const double invK[9], const double R[9], //current parameter (rotation and translation)
const double t[3],
int n2D3Ds, //number of 2D-3D correspondences
const double Ms[], //3D points
const double ms[], //2D points
int n2D2Ds, //number of 2D-2D correspondences
const double uRpre[], //camera poses at former frames
const double utpre[],
const double umspre[], //unmapped feature points at former frames
const double ums[], //unmapped feature points the current frames
double param[6], //new parameter
const double lambda // damping coefficient
) {
double sA[36], A[36], invSA[36], sB[6], B[6], rm[2], rerr[2], eerr;
double Jw[6], Jt[6];
memset(sA, 0, sizeof(double) * 36);
memset(sB, 0, sizeof(double) * 6);
const double* pMs = Ms;
const double* pms = ms;
for (int i = 0; i < n2D3Ds; i++, pMs += 3, pms += 2) {
project(K, R, t, pMs, rm);
_perspectiveSO3JacobiNum(K, R, t, pMs, pms, rm, Jw);
_perspectiveTJacobiNum(K, R, t, pMs, pms, rm, Jt);
double J[12] = { Jw[0], Jw[1], Jw[2], Jt[0], Jt[1], Jt[2], Jw[3], Jw[4], Jw[5], Jt[3], Jt[4], Jt[5] };
matATB(2, 6, 2, 6, J, J, A);
mat66sum(sA, A, sA);
rerr[0] = -rm[0] + pms[0];
rerr[1] = -rm[1] + pms[1];
matATB(2, 6, 2, 1, J, rerr, B);
mat16sum(sB, B, sB);
}
const double* pms1 = umspre;
const double* pms2 = ums;
const double* pRpre = uRpre;
const double* ptpre = utpre;
for (int j = 0; j < n2D2Ds; j++, pms1 += 2, pms2 += 2, pRpre += 9, ptpre += 3) {
//compute the epipolar error
double E[9], F[9], Ew[3], Et[3];
formEMat(R, t, pRpre, ptpre, E);
getFMat(invK, invK, E, F);
eerr = epipolarError(F, pms2, pms1);
_epiDistSO3JacobiNum(invK, pRpre, ptpre, pms1, R, t, pms2, eerr, Ew);
_epiDistTJacobiNum(invK, pRpre, ptpre, pms1, R, t, pms2, eerr, Et);
double J[6] = { Ew[0], Ew[1], Ew[2], Et[0], Et[1], Et[2] };
matATB(1, 6, 1, 6, J, J, A);
mat66sum(sA, A, sA);
B[0] = -J[0] * eerr;
B[1] = -J[1] * eerr;
B[2] = -J[2] * eerr;
B[3] = -J[3] * eerr;
B[4] = -J[4] * eerr;
B[5] = -J[5] * eerr;
mat16sum(sB, B, sB);
}
sA[0] += lambda;
sA[7] += lambda;
sA[14] += lambda;
sA[21] += lambda;
sA[28] += lambda;
sA[35] += lambda;
matInv(6, sA, invSA);
matAB(6, 6, 6, 1, invSA, sB, param);
}
