/* invert a matrix using Gaussian Elimination */
Matrix invertRREF(Matrix m) {
int prealloc = alloc_matrix;
int i,j;
Matrix tmp = makeZeroMatrix(m->row_dim, m->col_dim*2);
Matrix inverse = makeMatrix(m->row_dim, m->col_dim);
DEBUG_CHECK(m->row_dim == m->col_dim,"Matrix can only be inverted if it is square");
/* build the tmp Matrix which will be passed to RREF */
for( i = 0; i < m->row_dim; i++) {
for( j = 0; j < m->col_dim; j++) {
ME(tmp,i,j) = ME(m,i,j);
if(i == j) {
ME(tmp,i,j+m->col_dim) = 1;
}
}
}
matrixRREF(tmp);
for( i = 0; i < m->row_dim; i++) {
for( j = 0; j < m->col_dim; j++) {
ME(inverse,i,j) = ME(tmp,i,j+m->col_dim);
}
}
freeMatrix(tmp);
if(prealloc != alloc_matrix - 1) {
printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
exit(1);
}
return inverse;
}
MutableMatrix* MutableMat<T>::invert() const
{
MutableMat<T>* result = makeZeroMatrix(n_rows(), n_cols());
bool val = mat.invert(result->mat);
if (!val)
{
delete result;
return 0;
}
return result;
}
GraphDiscription buildBunchGraph(GraphDiscription modelJets, JetSimilarityMeasure jetSim, int bunchsize){
int vert;
assert(bunchsize > 1);
for(vert = 0; vert < modelJets->numVert; vert++){
Matrix dist;
int size, i, j;
FTYPE maxsim, minsim, smallest;
int minjet;
JetBunch finalBunch = makeJetBunch();
MESSAGE1ARG("Selecting jets for point: %s",modelJets->vertLabels[vert]);
size = modelJets->bunch[vert]->size;
assert(bunchsize <= size);
assert(size > 0);
dist = makeZeroMatrix(size, size);
smallest = maxsim = jetSim(modelJets->bunch[vert]->jets[0],modelJets->bunch[vert]->jets[0]);
for(i = 0; i < size; i++){
for(j = 0; j < size; j++){
ME(dist,i,j) = jetSim(modelJets->bunch[vert]->jets[i],modelJets->bunch[vert]->jets[j]);
maxsim = MAX( maxsim, ME(dist,i,j) );
smallest = MIN(smallest, ME(dist,i,j));
}
}
/* find the minimum similarity */
minsim = 0.0;
minjet = -1;
for(i = 0; i < size; i++){
for(j = 0; j < size; j++){
if(minjet < 0 || minsim > ME(dist,i,j) ){
minsim = ME(dist,i,j);
minjet = i;
}
}
}
while(1){
/* MESSAGE2ARG(" Adding jet %03d to final bunch. sim=%+f",minjet,minsim); */
/* add that jet to the final bunch */
addJetToBunch(finalBunch, modelJets->bunch[vert]->jets[minjet]);
/* eleminate the newly added jet from the old bunch and the distance matrix */
modelJets->bunch[vert]->jets[minjet] = NULL;
for( i = 0; i < size; i++){
/* Make sure new value is greater that the maximum similarity */
ME(dist,i,minjet) = 2*ABS(maxsim);
}
if(finalBunch->size >= bunchsize) break;
minsim = 0.0;
minjet = -1;
for(j = 0; j < size; j++){
FTYPE locmax = smallest;
for(i = 0; i < size; i++){
/* compute the best similarity to the jets in the data set */
if(modelJets->bunch[vert]->jets[i] == NULL){
locmax = MAX(locmax,ME(dist,i,j));
}
}
if(minjet < 0 || minsim > locmax ){
minsim = locmax;
minjet = j;
}
}
}
freeJetBunch(modelJets->bunch[vert]);
modelJets->bunch[vert] = finalBunch;
freeMatrix(dist);
}
return modelJets;
}
void
computeConsistentRotations_LSQ(int const nIterations, int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations)
{
double const gamma = 1.0;
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
Matrix3x3d zero3x3d;
makeZeroMatrix(zero3x3d);
Matrix4x4d zeroQuat;
makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;
vector<double> denomQ(nViews, 1.0); // from the psd constraint
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
denomQ[i] += 1;
denomQ[j] += 1;
}
for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];
vector<Matrix4x4d> Q(nViews, zeroQuat);
vector<Matrix4x4d> Q1(nViews, zeroQuat);
vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);
vector<Matrix3x3d> A(nRelPoses, zero3x3d);
vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);
for (int iter = 0; iter < nIterations; ++iter)
{
// Convex hull of rotation matrices
for (int i = 0; i < nViews; ++i)
{
Matrix4x4d q = Q[i] + ZQ1[i];
projectConvHull_SO3(q);
Q1[i] = q;
addMatricesIP(Q[i] - q, ZQ1[i]);
} // end for (i)
// Squared Frobenius term 0.5*sum_k |A_k|_F^2
for (int k = 0; k < nRelPoses; ++k)
{
Matrix3x3d a = A[k] + ZA1[k];
scaleMatrixIP(1.0 / (1.0 + gamma), a);
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];
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)
{
double E = 0;
for (int k = 0; k < nRelPoses; ++k) E += sqrMatrixNormFrobenius(A[k]);
cout << "iter: " << iter << " E = " << E << endl;
}
} // end for (iter)
rotations.resize(nViews);
for (int i = 0; i < nViews; ++i)
rotations[i] = getRotationFromQuat(Q[i]);
} // end computeConsistentRotations_LSQ()
void
computeConsistentRotations_L1(double const sigma, int const nIterations, int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations)
{
double const gamma = 1.0;
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
Matrix3x3d zero3x3d;
makeZeroMatrix(zero3x3d);
Matrix4x4d zeroQuat;
makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;
vector<double> denomQ(nViews, 1.0); // from the psd constraint
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
denomQ[i] += 1;
denomQ[j] += 1;
}
for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];
vector<double> T(nRelPoses, 0.0);
vector<double> T1(nRelPoses);
vector<double> ZT1(nRelPoses, 0.0);
vector<double> T2(nRelPoses);
vector<double> ZT2(nRelPoses, 0.0);
vector<Matrix4x4d> Q(nViews, zeroQuat);
vector<Matrix4x4d> Q1(nViews, zeroQuat);
vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);
vector<Matrix3x3d> A(nRelPoses, zero3x3d);
vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);
for (int iter = 0; iter < nIterations; ++iter)
{
// Convex hull of rotation matrices
for (int i = 0; i < nViews; ++i)
{
Matrix4x4d q = Q[i] + ZQ1[i];
projectConvHull_SO3(q);
Q1[i] = q;
addMatricesIP(Q[i] - q, ZQ1[i]);
} // end for (i)
// Shrinkage of T (we want to minimize T)
for (int k = 0; k < nRelPoses; ++k)
{
T2[k] = std::max(0.0, T[k] + ZT2[k] - gamma);
ZT2[k] += T[k] - T2[k];
} // end for (k)
// Cone constraint
for (int k = 0; k < nRelPoses; ++k)
{
double t = T1[k] + ZT1[k];
Matrix3x3d a = A[k] + ZA1[k];
proxDataResidual_Frobenius(sigma, t, a);
T1[k] = t;
ZT1[k] += T[k] - 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];
for (int k = 0; k < nRelPoses; ++k)
T[k] = std::max(0.0, 0.5 * (T1[k] - ZT1[k] + T2[k] - ZT2[k]));
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 << endl;
cout << " T = "; displayVector(T);
}
} // end for (iter)
rotations.resize(nViews);
for (int i = 0; i < nViews; ++i)
rotations[i] = getRotationFromQuat(Q[i]);
} // end computeConsistentRotations_L1()
void
computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations, std::vector<double>& zs)
{
double const gamma = 1.0;
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
Matrix3x3d zero3x3d;
makeZeroMatrix(zero3x3d);
Matrix4x4d zeroQuat;
makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;
double const denomT = 1.0 / (1.0 + nRelPoses);
vector<double> denomQ(nViews, 1.0); // from the psd constraint
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
denomQ[i] += 1;
denomQ[j] += 1;
}
for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];
double T = 0.0;
vector<double> T1(nRelPoses);
vector<double> ZT1(nRelPoses, 0.0);
double T2;
double ZT2 = 0;
vector<Matrix4x4d> Q(nViews, zeroQuat);
vector<Matrix4x4d> Q1(nViews, zeroQuat);
vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);
vector<Matrix3x3d> A(nRelPoses, zero3x3d);
vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);
for (int iter = 0; iter < nIterations; ++iter)
{
// Convex hull of rotation matrices
for (int i = 0; i < nViews; ++i)
{
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()
int main(int argc, char** argv) {
ImageList *imagenames, *subject, *replicate;
int imageCount;
Matrix distance;
FaceGraph* graphs;
char** names;
int i, j;
Arguments args;
DistDirNode* distrec;
processCommand(argc, argv, &args);
MESSAGE("Reading in image names");
imagenames = getImageNames(args.imageNamesFile, &imageCount);
MESSAGE1ARG("Reading in graph files %d",imageCount);
/* Allocate space for imagenames, face graphs and distance matrix */
names = ALLOCATE(char*,imageCount);
graphs = ALLOCATE(FaceGraph, imageCount);
distance = makeZeroMatrix(imageCount,imageCount);
MESSAGE("Reading in graph files");
i = 0;
for(subject = imagenames; subject != NULL; subject = subject->next_subject) {
for( replicate = subject; replicate != NULL; replicate = replicate->next_replicate) {
printf("Reading in graph: %s\n", replicate->filename);
fflush(stdout);
names[i] = strdup(replicate->filename);
graphs[i] = loadFaceGraph(makePath(args.faceGraphDir,replicate->filename));
i++;
}
}
for(distrec = args.distList; distrec != NULL; distrec = distrec->next) {
/* Create distance matrix */
completed = 0;
total = imageCount*imageCount;
MESSAGE("Computing Distance Matrix");
start_time = time(NULL);
computeDistanceMatrix(distance, graphs, 0, imageCount, 0, imageCount, distrec->distMeasure);
/* Print out distance files to the distance directory */
for(i = 0; i < imageCount; i++) {
FILE* distfile = fopen(makePath(distrec->distDirectory,names[i]), "w");
if(!distfile) {
printf("Error opening distance file: %s\n",makePath(distrec->distDirectory,names[i]));
exit(1);
}
printf("Saving distances for image: %s\n",names[i]);
fflush(stdout);
for(j = 0; j < imageCount; j++) {
fprintf(distfile,"%s\t%16e\n",names[j], ME(distance,i,j));
}
fclose(distfile);
}
}
return 0;
}
Matrix3x3d
computeIntersectionCovariance(vector<Matrix3x4d> const& projections,
vector<PointMeasurement> const& measurements,
double sigma)
{
Matrix<double> Cp(3, 3, 0.0);
Cp[0][0] = Cp[1][1] = sigma;
Cp[2][2] = 0.0;
int const N = measurements.size();
Matrix<double> A(2*N, 4, 0.0);
InlineMatrix<double, 2, 3> Sp;
makeZeroMatrix(Sp);
InlineMatrix<double, 2, 4> Sp_P;
for (int i = 0; i < N; ++i)
{
Sp[0][1] = -1; Sp[0][2] = measurements[i].pos[1];
Sp[1][0] = 1; Sp[1][2] = -measurements[i].pos[0];
int const view = measurements[i].view;
multiply_A_B(Sp, projections[view], Sp_P);
A[2*i+0][0] = Sp_P[0][0]; A[2*i+0][1] = Sp_P[0][1]; A[2*i+0][2] = Sp_P[0][2]; A[2*i+0][3] = Sp_P[0][3];
A[2*i+1][0] = Sp_P[1][0]; A[2*i+1][1] = Sp_P[1][1]; A[2*i+1][2] = Sp_P[1][2]; A[2*i+1][3] = Sp_P[1][3];
} // end for (i)
SVD<double> svd(A);
Matrix<double> V;
svd.getV(V);
Vector4d X;
X[0] = V[0][3]; X[1] = V[1][3]; X[2] = V[2][3]; X[3] = V[3][3];
Vector3d P;
Matrix<double> S(2, 3, 0.0);
Matrix<double> B(2*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
int const view = measurements[i].view;
multiply_A_v(projections[view], X, P);
P[0] /= P[2]; P[1] /= P[2]; P[2] = 1.0;
S[0][1] = -P[2]; S[0][2] = P[1];
S[1][0] = P[2]; S[1][2] = -P[0];
B[2*i+0][3*i+0] = -S[0][0]; B[2*i+0][3*i+1] = -S[0][1]; B[2*i+0][3*i+2] = S[0][2];
B[2*i+1][3*i+0] = -S[1][0]; B[2*i+1][3*i+1] = -S[1][1]; B[2*i+1][3*i+2] = S[1][2];
} // end for (i)
Matrix<double> C(3*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
C[3*i+0][3*i+0] = Cp[0][0]; C[3*i+0][3*i+1] = Cp[0][1]; C[3*i+0][3*i+2] = Cp[0][2];
C[3*i+1][3*i+0] = Cp[1][0]; C[3*i+1][3*i+1] = Cp[1][1]; C[3*i+1][3*i+2] = Cp[1][2];
C[3*i+2][3*i+0] = Cp[2][0]; C[3*i+2][3*i+1] = Cp[2][1]; C[3*i+2][3*i+2] = Cp[2][2];
} // end for (i)
Matrix<double> B_C(2*N, 3*N);
multiply_A_B(B, C, B_C);
Matrix<double> T(2*N, 2*N);
multiply_A_Bt(B_C, B, T);
Matrix<double> Tinv;
invertMatrix(T, Tinv);
Matrix<double> NN(5, 5), N4(4, 4);
Matrix<double> At_Tinv(4, 2*N);
multiply_At_B(A, Tinv, At_Tinv);
multiply_A_B(At_Tinv, A, N4);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
NN[r][c] = N4[r][c];
NN[0][4] = NN[4][0] = X[0];
NN[1][4] = NN[4][1] = X[1];
NN[2][4] = NN[4][2] = X[2];
NN[3][4] = NN[4][3] = X[3];
NN[4][4] = 0.0;
Matrix<double> Ninv(5, 5);
invertMatrix(NN, Ninv);
Matrix4x4d sigma_XX;
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
sigma_XX[r][c] = Ninv[r][c];
Matrix3x4d Je;
makeZeroMatrix(Je);
Je[0][0] = Je[1][1] = Je[2][2] = 1.0 / X[3];
Je[0][3] = -X[0] / (X[3]*X[3]);
Je[1][3] = -X[1] / (X[3]*X[3]);
Je[2][3] = -X[2] / (X[3]*X[3]);
Matrix3x3d sigma_X = Je * sigma_XX * Je.transposed();
return sigma_X;
}
