Skip to content

Commit ffe990d

Browse files
jmeidowjmeidow
committed
matrix functions bundled in matfun.h
1 parent 7261ff5 commit ffe990d

File tree

8 files changed

+93
-140
lines changed

8 files changed

+93
-140
lines changed

‎src/adjustment.cpp‎

Lines changed: 1 addition & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include "adjustment.h"
2020
#include "constraints.h"
2121
#include "global.h"
22+
#include "matfun.h"
2223
#include "matrix.h"
2324

2425
#include <QString>
@@ -27,11 +28,9 @@
2728
#include "qassert.h"
2829
#include "qcontainerfwd.h"
2930
#include "qlogging.h"
30-
#include "qtdeprecationdefinitions.h"
3131

3232
#include <algorithm>
3333
#include <cassert>
34-
#include <cfloat>
3534
#include <cmath>
3635
#include <memory>
3736
#include <numeric>
@@ -67,48 +66,6 @@ Index AdjustmentFramework::indexOf(const Eigen::VectorXi &v, const Index i)
6766
return std::distance( v.begin(), it); */
6867
}
6968

70-
MatrixXd AdjustmentFramework::Rot_ab(const VectorXd &a, const VectorXd &b)
71-
{
72-
Q_ASSERT( a.size()==b.size());
73-
#ifdef QT_DEBUG
74-
Q_ASSERT( fabs( a.norm()-1.) < FLT_EPSILON );
75-
Q_ASSERT( fabs( b.norm()-1.) < FLT_EPSILON );
76-
#endif
77-
return MatrixXd::Identity( a.size(),a.size())
78-
+2*b*a.adjoint()
79-
-(a+b)*(a+b).adjoint()/(1.+a.dot(b));
80-
}
81-
82-
MatrixXd AdjustmentFramework::null(const VectorXd &xs)
83-
{
84-
// cf. PCV, eq. (A.120)
85-
86-
//if ( fabs(xs.norm()-1.) > T_ZERO )
87-
// qDebug() << xs;
88-
89-
#ifdef QT_DEBUG
90-
QString const what = QStringLiteral("norm(x) = %1").arg(QString::number(xs.norm()));
91-
Q_ASSERT_X(std::fabs(xs.norm() - 1.) <= FLT_EPSILON, Q_FUNC_INFO, what.toStdString().data());
92-
#endif
93-
Eigen::Index const N = xs.size();
94-
95-
VectorXd x0 = xs.head(N-1);
96-
double xN = xs(N-1);
97-
if ( xN < 0.0 ) {
98-
x0 = -x0;
99-
xN = -xN;
100-
}
101-
102-
MatrixXd JJ( N, N-1);
103-
JJ.topRows(N-1) = MatrixXd::Identity(N-1,N-1) -x0*x0.adjoint()/(1.+xN);
104-
JJ.bottomRows(1) = -x0.adjoint();
105-
106-
VectorXd const check = JJ.adjoint()*xs;
107-
#ifdef QT_DEBUG
108-
Q_ASSERT_X( check.norm() <= FLT_EPSILON, Q_FUNC_INFO, "not a zero vector");
109-
#endif
110-
return JJ;
111-
}
11269

11370
std::pair<VectorXd,MatrixXd >
11471
AdjustmentFramework::getEntity( const Index s,
@@ -433,10 +390,3 @@ void AdjustmentFramework::check_constraints(
433390
}
434391
}
435392

436-
//! check if the matrix AA is rank-deficient
437-
bool AdjustmentFramework::is_rank_deficient( Eigen::SparseMatrix<double,Eigen::ColMajor> & AA, const double T )
438-
{
439-
Eigen::ColPivHouseholderQR<MatrixXd> qr(AA);
440-
qr.setThreshold( T );
441-
return ( qr.rank() < AA.rows() );
442-
}

‎src/adjustment.h‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -107,13 +107,14 @@ class AdjustmentFramework
107107
static constexpr double rankEstimate = 1e-6; // rank estimation
108108
static constexpr double numericalCheck = 1e-5; // numerical check constraints
109109

110-
110+
/*
111111
//! Rotation matrix for minimal rotation
112112
static Eigen::MatrixXd Rot_ab(const Eigen::VectorXd &a, const Eigen::VectorXd &b);
113113
//! Nullspace of row vector
114114
static Eigen::MatrixXd null(const Eigen::VectorXd &xs);
115+
[[nodiscard]] static bool is_rank_deficient( Eigen::SparseMatrix<double,Eigen::ColMajor> & AA, double T );
116+
*/
115117

116-
[[nodiscard]] static bool is_rank_deficient( Eigen::SparseMatrix<double,Eigen::ColMajor> & BBr, double T );
117118
static Eigen::Index indexOf(const Eigen::VectorXi &v, Eigen::Index i);
118119
};
119120

‎src/constraints.cpp‎

Lines changed: 1 addition & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -24,13 +24,12 @@
2424
#include <QStringLiteral>
2525

2626
#include "qassert.h"
27-
#include "qtdeprecationdefinitions.h"
2827

2928
#include <cassert>
30-
#include <cfloat>
3129
#include <cmath>
3230
#include <cstdlib>
3331

32+
#include "matfun.h"
3433

3534
namespace Constraint {
3635

@@ -39,51 +38,6 @@ using Eigen::Vector2cd;
3938
using Eigen::Matrix;
4039

4140

42-
MatrixXd ConstraintBase::Rot_ab( const VectorXd &a,
43-
const VectorXd &b)
44-
{
45-
Q_ASSERT( a.size()==b.size() );
46-
#ifdef QT_DEBUG
47-
Q_ASSERT( std::fabs( a.norm()-1.) < FLT_EPSILON );
48-
Q_ASSERT( std::fabs( b.norm()-1.) < FLT_EPSILON );
49-
#endif
50-
return MatrixXd::Identity( a.size(),a.size())
51-
+2*b*a.adjoint()
52-
-(a+b)*(a+b).adjoint()/(1.+a.dot(b));
53-
}
54-
55-
MatrixXd ConstraintBase::null( const VectorXd & xs )
56-
{
57-
// cf. PCV, eq. (A.120)
58-
59-
//if ( fabs(xs.norm()-1.) > T_ZERO )
60-
// qDebug() << xs;
61-
62-
#ifdef QT_DEBUG
63-
QString const what = QStringLiteral("norm(x) = %1").arg( QString::number(xs.norm()) );
64-
Q_ASSERT_X(fabs(xs.norm() - 1.) <= FLT_EPSILON, "null(x)", what.toStdString().data());
65-
#endif
66-
67-
Eigen::Index const N = xs.size();
68-
69-
VectorXd x0 = xs.head(N-1);
70-
double xN = xs(N-1);
71-
if ( xN < 0.0 ) {
72-
x0 = -x0;
73-
xN = -xN;
74-
}
75-
76-
MatrixXd JJ( N, N-1);
77-
JJ.topRows(N-1) = MatrixXd::Identity(N-1,N-1) -x0*x0.adjoint()/(1.+xN);
78-
JJ.bottomRows(1) = -x0.adjoint();
79-
80-
VectorXd const check = JJ.adjoint() * xs;
81-
#ifdef QT_DEBUG
82-
Q_ASSERT_X( check.norm() <= FLT_EPSILON, Q_FUNC_INFO, "not a zero vector");
83-
#endif
84-
return JJ;
85-
}
86-
8741
ConstraintBase::ConstraintBase()
8842
: m_status(UNEVAL)
8943
, m_enforced(false)

‎src/constraints.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <memory>
2828

2929
#include "cstdint"
30+
3031
#include "matrix.h"
3132

3233
//! Geometric constraints (relations)
@@ -52,9 +53,9 @@ class ConstraintBase
5253
virtual ~ConstraintBase() = default;
5354

5455
protected:
55-
static MatrixXd null( const VectorXd &xs ); //!< Nullspace of row vector
56-
static MatrixXd Rot_ab( const VectorXd &a,
57-
const VectorXd &b); //!< Minimal rotation between two vectors as rotation matrix
56+
// static MatrixXd null( const VectorXd &xs ); //!< Nullspace of row vector
57+
//static MatrixXd Rot_ab( const VectorXd &a,
58+
// const VectorXd &b); //!< Minimal rotation between two vectors as rotation matrix
5859
public:
5960
//! Status: unevaluated, required, obsolete (redundant)
6061
enum Status : std::uint8_t { UNEVAL=0, REQUIRED, OBSOLETE };

‎src/greasepad.pro‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@ HEADERS += \
6060
mainwindow.h \
6161
mainscene.h \
6262
mainview.h \
63+
matfun.h \
6364
matrix.h \
6465
qconstraints.h \
6566
qformattool.h \

‎src/matfun.h‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
#ifndef MATFUN_H
2+
#define MATFUN_H
3+
4+
5+
#include <Eigen/Core>
6+
#include <Eigen/QR>
7+
#include <Eigen/SparseCore>
8+
9+
#include <cfloat>
10+
#include <cmath>
11+
12+
#include <QString>
13+
#include <QStringLiteral>
14+
#include <qassert.h>
15+
16+
17+
namespace {
18+
19+
using Eigen::MatrixXd;
20+
using Eigen::VectorXd;
21+
using Eigen::SparseMatrix;
22+
23+
24+
//! Nullspace of row vector
25+
MatrixXd null( const VectorXd & xs )
26+
{
27+
// cf. PCV, eq. (A.120)
28+
29+
#ifdef QT_DEBUG
30+
QString const what = QStringLiteral("norm(x) = %1").arg( QString::number(xs.norm()) );
31+
Q_ASSERT_X(fabs(xs.norm() - 1.) <= FLT_EPSILON, "null(x)", what.toStdString().data());
32+
#endif
33+
34+
Eigen::Index const N = xs.size();
35+
36+
VectorXd x0 = xs.head(N-1);
37+
double xN = xs(N-1);
38+
if ( xN < 0.0 ) {
39+
x0 = -x0;
40+
xN = -xN;
41+
}
42+
43+
MatrixXd JJ( N, N-1);
44+
JJ.topRows(N-1) = MatrixXd::Identity(N-1,N-1) -x0*x0.adjoint()/(1.+xN);
45+
JJ.bottomRows(1) = -x0.adjoint();
46+
47+
VectorXd const check = JJ.adjoint() * xs;
48+
#ifdef QT_DEBUG
49+
Q_ASSERT_X( check.norm() <= FLT_EPSILON, Q_FUNC_INFO, "not a zero vector");
50+
#endif
51+
return JJ;
52+
}
53+
54+
55+
56+
MatrixXd Rot_ab( const VectorXd &a, const VectorXd &b)
57+
{
58+
Q_ASSERT( a.size()==b.size() );
59+
#ifdef QT_DEBUG
60+
Q_ASSERT( std::fabs( a.norm()-1.) < FLT_EPSILON );
61+
Q_ASSERT( std::fabs( b.norm()-1.) < FLT_EPSILON );
62+
#endif
63+
return MatrixXd::Identity( a.size(),a.size())
64+
+2*b*a.adjoint()
65+
-(a+b)*(a+b).adjoint()/(1.+a.dot(b));
66+
}
67+
68+
69+
//! check if the matrix AA is rank-deficient
70+
bool is_rank_deficient( Eigen::SparseMatrix<double,Eigen::ColMajor> & AA, const double T )
71+
{
72+
Eigen::ColPivHouseholderQR<MatrixXd> qr(AA);
73+
qr.setThreshold( T );
74+
return ( qr.rank() < AA.rows() );
75+
}
76+
77+
} // namespace
78+
79+
80+
#endif // MATFUN_H

‎src/uncertain.cpp‎

Lines changed: 2 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -18,15 +18,14 @@
1818

1919
#include "uncertain.h"
2020
#include "qlogging.h"
21+
22+
#include "matfun.h"
2123
#include "usegment.h"
2224
#include "ustraightline.h"
2325

2426
#include <QDebug>
2527
#include <QStringLiteral>
2628

27-
#include "qassert.h"
28-
#include "qtdeprecationdefinitions.h"
29-
3029
#include <cassert>
3130
#include <cfloat>
3231
#include <cmath>
@@ -36,39 +35,6 @@
3635

3736
namespace Uncertain {
3837

39-
//! Nullspace of row vector
40-
Matrix<double,3,2> BasicEntity2D::null( const Vector3d &xs )
41-
{
42-
// cf. PCV, eq. (A.120)
43-
44-
//if ( fabs(xs.norm()-1.) > T_ZERO )
45-
// qDebug() << xs;
46-
47-
#ifdef QT_DEBUG
48-
QString const what = QStringLiteral("norm(x) = %1").arg(QString::number(xs.norm()));
49-
Q_ASSERT_X(std::fabs(xs.norm() - 1.) <= FLT_EPSILON, Q_FUNC_INFO, what.toStdString().data());
50-
#endif
51-
Eigen::Index const N = xs.size();
52-
53-
VectorXd x0 = xs.head(N-1);
54-
double xN = xs(N-1);
55-
if ( xN < 0.0 ) {
56-
x0 = -x0;
57-
xN = -xN;
58-
}
59-
60-
MatrixXd JJ( N, N-1);
61-
JJ.topRows(N-1) = MatrixXd::Identity(N-1,N-1) -x0*x0.adjoint()/(1.+xN);
62-
JJ.bottomRows(1) = -x0.adjoint();
63-
64-
const VectorXd check = JJ.adjoint()*xs;
65-
Q_ASSERT_X( check.norm() <= FLT_EPSILON,
66-
Q_FUNC_INFO,
67-
"not a zero vector");
68-
69-
return JJ;
70-
}
71-
7238

7339

7440
//! Check if matrix MM is a proper covariance matrix

‎src/uncertain.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,7 @@ class BasicEntity2D
103103
Matrix3d m_cov; //!< homogeneous 3x3 covariance matrix
104104

105105
//! Nullspace of row 3-vector
106-
static Eigen::Matrix<double, 3, 2> null(const Eigen::Vector3d &xs);
106+
//static Eigen::Matrix<double, 3, 2> null(const Eigen::Vector3d &xs);
107107
};
108108

109109
} // namespace Uncertain

0 commit comments

Comments
 (0)