From ba0abeed1c0ec2aca28298a76f844f0ddb556fa2 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 17 Dec 2021 18:42:37 +0100
Subject: [PATCH] wip
---
sm/lib/gpc/gpc.c | 12 ++++--------
sm/lib/gpc/gpc_utils.c | 22 ++++++++++++++++++++--
sm/lib/gpc/gpc_utils.h | 4 +++-
3 files changed, 27 insertions(+), 11 deletions(-)
diff --git a/sm/lib/gpc/gpc.c b/sm/lib/gpc/gpc.c
index da8250d..fd99cf2 100644
--- a/sm/lib/gpc/gpc.c
+++ b/sm/lib/gpc/gpc.c
@@ -131,12 +131,11 @@ int gpc_solve(int K, const struct gpc_corr*c,
/* mS = mD - mB.trans * mA.inv * mB;
temp22b = inv(A) */
- if ( singular(mA) )
+ if ( m_inv(mA, temp22b) == 0 )
{
sm_error("gpc_solve: mA is singular!\n");
return 0;
}
- m_inv(mA, temp22b);
/* temp22c = inv(A) * mB */
m_mult(temp22b, mB, temp22c);
/* temp22 = mB' */
@@ -146,12 +145,11 @@ int gpc_solve(int K, const struct gpc_corr*c,
m_add(mD,temp22b,mS);
/* mSa = mS.inv * mS.det; */
- if ( singular(mS) )
+ if ( m_inv(mS, mSa) == 0 )
{
sm_error("gpc_solve: mS is singular!\n");
return 0;
}
- m_inv(mS, mSa);
m_scale(m_det(mS), mSa);
if(TRACE_ALGO) {
@@ -173,12 +171,11 @@ int gpc_solve(int K, const struct gpc_corr*c,
m_trans(g1, g1t);
m_trans(g2, g2t);
m_trans(mB, mBt);
- if ( singular(mA) )
+ if ( m_inv(mA, mAi) == 0 )
{
sm_error("gpc_solve: mA is singular!\n");
return 0;
}
- m_inv(mA, mAi);
M(m1t,1,2); M(m1,2,1);
M(m2t,1,2); M(m2,2,1);
@@ -270,12 +267,11 @@ int gpc_solve(int K, const struct gpc_corr*c,
m_scale(2*lambda, W);
gsl_matrix_add(bigM,W);
- if ( singular(bigM) )
+ if ( m_inv(bigM, temp44) == 0 )
{
sm_error("gpc_solve: bigM is singular!\n");
return 0;
}
- m_inv(bigM, temp44);
m_mult(temp44, g, x);
m_scale(-1.0, x);
diff --git a/sm/lib/gpc/gpc_utils.c b/sm/lib/gpc/gpc_utils.c
index 0014ac1..5a4b23a 100644
--- a/sm/lib/gpc/gpc_utils.c
+++ b/sm/lib/gpc/gpc_utils.c
@@ -22,7 +22,7 @@ void m_add (const gsl_matrix*A, const gsl_matrix*B, gsl_matrix*ApB){
gsl_matrix_add(ApB,B);
}
-void m_inv(const gsl_matrix*A, gsl_matrix*invA) {
+int m_inv(const gsl_matrix*A, gsl_matrix*invA) {
unsigned int n = A->size1;
gsl_matrix * m = gsl_matrix_alloc(n,n);
gsl_matrix_memcpy(m,A);
@@ -33,12 +33,15 @@ void m_inv(const gsl_matrix*A, gsl_matrix*invA) {
if ( singular (m))
{
fprintf(stderr, "m_inv: m is singular!\n");
- return;
+ print_mat(A);
+ return 0;
}
/* Invert the matrix m */
gsl_linalg_LU_invert (m, perm, invA);
gsl_permutation_free(perm);
gsl_matrix_free(m);
+
+ return 1;
}
double m_det(const gsl_matrix*A) {
@@ -149,3 +152,18 @@ int singular (const gsl_matrix * LU)
return 0;
}
+
+void print_mat(const gsl_matrix * LU)
+{
+ size_t i, j, n = LU->size1, m = LU->size2;
+
+ for (i = 0; i < n; i++)
+ {
+ for (j = 0; j < m; j++)
+ {
+ printf("\t%.5f", gsl_matrix_get(LU, i, j));
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
diff --git a/sm/lib/gpc/gpc_utils.h b/sm/lib/gpc/gpc_utils.h
index 4878bfa..07a09c2 100644
--- a/sm/lib/gpc/gpc_utils.h
+++ b/sm/lib/gpc/gpc_utils.h
@@ -18,7 +18,7 @@ void m_add(const gsl_matrix*A, const gsl_matrix*B, gsl_matrix*ApB);
void m_add_to(const gsl_matrix*A, gsl_matrix*B);
void m_scale(double m, gsl_matrix*A);
double m_dot(const gsl_matrix*A,const gsl_matrix*B);
-void m_inv(const gsl_matrix*A, gsl_matrix*invA);
+int m_inv(const gsl_matrix*A, gsl_matrix*invA);
double m_det(const gsl_matrix*A);
/* Returns the real part of the roots in roots */
@@ -30,4 +30,6 @@ void m_display(const char*str, gsl_matrix*m);
int singular (const gsl_matrix * LU);
+void print_mat(const gsl_matrix * LU);
+
#endif
--
GitLab