Skip to content

Commit 3d71cc1

Browse files
jmeidowjmeidow
committed
unary constraints introduced (vertical, horizontal, diagonal)
1 parent e3a5db5 commit 3d71cc1

33 files changed

+2390
-635
lines changed

‎src/adjustment.cpp‎

Lines changed: 56 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by
@@ -107,6 +107,7 @@ AdjustmentFramework::getEntity( const Index s,
107107
}
108108

109109

110+
//! update of parameters (observations) via retraction
110111
void AdjustmentFramework::update( const Index start,
111112
const VectorXd &x)
112113
{
@@ -118,7 +119,7 @@ void AdjustmentFramework::update( const Index start,
118119
// m.segment(idx3,3).normalize();
119120

120121
// (2) via retraction ......................................................
121-
Eigen::Vector3d v = null( l0_.segment(idx3,3) )*x.segment(2*start,2);
122+
Eigen::Vector3d v = null( l0_.segment(idx3,3) )*x.segment(2*start,2);
122123
double nv = v.norm();
123124
if ( nv<=0.0 ) {
124125
return;
@@ -142,22 +143,30 @@ bool AdjustmentFramework::enforce_constraints( const QVector<std::shared_ptr<Con
142143
const Index C = mapc.size();
143144
const Index S = maps.size();
144145

146+
// number of required constraints, inclusive hypothesis to be tested
147+
int numReq = 0;
148+
for ( Index c=0; c<mapc.size(); c++ ) {
149+
if ( constr->at( mapc(c) )->required() ) {
150+
numReq++;
151+
}
152+
}
145153
if ( verbose ) {
146-
qDebug().noquote() << QString("Trying to enforce %1 constraint%2...")
154+
qDebug().noquote() << QString("%1 constraint%2 recognized...")
147155
.arg( C). arg( C==1 ? "" : "s" );
156+
qDebug().noquote() << QString("%1 constraint%2 required?...")
157+
.arg( numReq). arg( numReq==1 ? "" : "s" );
148158
}
149159

150160
reset(); // set adjusted observations l0 := l
151161

152-
// number of equations (not constraints),
162+
// number of required equations (not constraints!),
153163
// e.g., two equations fpr parallelism
154164
int E = 0;
155165
for ( Index c=0; c<mapc.size(); c++ ) {
156166
if ( constr->at( mapc(c) )->required() ) {
157167
E += constr->at( mapc(c) )->dof();
158168
}
159169
}
160-
// assert( E==E2 ); // TODO
161170

162171
VectorXd redl;
163172
VectorXd cg;
@@ -177,7 +186,7 @@ bool AdjustmentFramework::enforce_constraints( const QVector<std::shared_ptr<Con
177186
if ( verbose ) {
178187
qDebug().noquote() << QString(" iteration #%1...").arg(it+1);
179188
}
180-
a_Jacobian( constr, Bi, BBr, g0, maps, mapc);
189+
Jacobian( constr, Bi, BBr, g0, maps, mapc);
181190

182191
// reduced coordinates: vector and covariance matrix .............
183192
for ( Index s=0; s<S; s++ )
@@ -191,8 +200,9 @@ bool AdjustmentFramework::enforce_constraints( const QVector<std::shared_ptr<Con
191200
lr.segment(offset2,2) = NN.adjoint() * l_segment(offset3,3);
192201

193202
// (ii) covariance matrix, reduced coordinates
194-
Eigen::Matrix3d RR = Rot_ab( l_segment(offset3,3),
195-
l0_segment(offset3,3) );
203+
Eigen::Matrix3d RR = Rot_ab(
204+
l_segment(offset3,3),
205+
l0_segment(offset3,3) );
196206
Eigen::Matrix<double,2,3> JJ = NN.adjoint()*RR;
197207
Eigen::Matrix2d Cov_rr = JJ*Cov_ll_block( offset3,3)*JJ.adjoint();
198208

@@ -212,7 +222,9 @@ bool AdjustmentFramework::enforce_constraints( const QVector<std::shared_ptr<Con
212222
if ( rcn < threshold_ReciprocalConditionNumber() ) {
213223
// redundant or contradictory constraint
214224
if ( verbose ) {
215-
qDebug().noquote() << QString("-> ill-conditioned. Reciprocal condition number = %1").arg(rcn);
225+
qDebug().noquote() << red
226+
<< QString("-> ill-conditioned. Reciprocal condition number = %1").arg(rcn)
227+
<< black;
216228
}
217229
return false;
218230
}
@@ -246,32 +258,31 @@ bool AdjustmentFramework::enforce_constraints( const QVector<std::shared_ptr<Con
246258
} // loop iterations
247259

248260
// check convergence ........................................
249-
if ( redl.norm() >= threshold_convergence() )
250-
{
261+
if ( redl.norm() >= threshold_convergence() ) {
251262
// redundant or contradictory
252263
if ( verbose ) {
253-
qDebug().noquote() << QString("-> Not converged after %1 iteration%2. Reciprocal condition number = %3")
264+
qDebug().noquote() << red << QString("-> Not converged after %1 iteration%2. Reciprocal condition number = %3")
254265
.arg( nIterMax() )
255266
.arg( nIterMax()==1 ? "" : "s")
256-
.arg( rcn );
267+
.arg( rcn ) << black;
257268
}
258269
return false;
259270
}
260271
if ( verbose ) {
261-
qDebug().noquote() << QString("-> Converged after %1 iteration%2. Reciprocal condition number = %3")
272+
qDebug().noquote() << green << QString("-> Converged after %1 iteration%2. Reciprocal condition number = %3")
262273
.arg( it)
263274
.arg( it+1==1 ? "" : "s")
264-
.arg( rcn );
275+
.arg( rcn ) << black;
265276
}
266277

267278
// check constraints .........................................
268-
a_check_constraints( constr, Bi, maps, mapc);
279+
check_constraints( constr, Bi, maps, mapc);
269280

270281
return true;
271282
}
272283

273284

274-
void AdjustmentFramework::a_Jacobian(
285+
void AdjustmentFramework::Jacobian(
275286
const QVector<std::shared_ptr<Constraint::ConstraintBase> > *constr,
276287
const IncidenceMatrix * Bi,
277288
SparseMatrix<double, Eigen::ColMajor> & BBr,
@@ -301,8 +312,7 @@ void AdjustmentFramework::a_Jacobian(
301312

302313
auto JJ = con->Jacobian( idx, l0(), l() );
303314
int dof = con->dof();
304-
for ( int i=0; i< con->arity(); i++ )
305-
{
315+
for ( int i=0; i< con->arity(); i++ ) {
306316
// dof rows for this constraint
307317
Q_ASSERT( JJ.rows()==dof );
308318
for ( Index r=0; r<JJ.rows(); r++ ) {
@@ -316,7 +326,8 @@ void AdjustmentFramework::a_Jacobian(
316326

317327
}
318328

319-
void AdjustmentFramework::a_check_constraints(
329+
//! check constraints (required and non-required)
330+
void AdjustmentFramework::check_constraints(
320331
const QVector<std::shared_ptr<Constraint::ConstraintBase> > *constr,
321332
const IncidenceMatrix * bi,
322333
const Eigen::RowVectorXi & maps,
@@ -329,16 +340,24 @@ void AdjustmentFramework::a_check_constraints(
329340
#ifdef QT_DEBUG
330341
const Index S = maps.size();
331342
for (int s=0; s<S; s++) {
343+
if (verbose) {
344+
qDebug().noquote()
345+
<< QString("straight line #%1: [").arg(s+1, 2)
346+
<< l0_segment(3*s,3).x() << ","
347+
<< l0_segment(3*s,3).y() << ","
348+
<< l0_segment(3*s,3).z() << "], \tnorm ="
349+
<< l0_segment(3*s,3).norm();
350+
}
332351
d = 1.0 -l0_segment(3*s,3).norm();
333352
if ( std::fabs( d ) > threshold_numericalCheck() ) {
334353
// QApplication::beep();
335-
qDebug().noquote() << QString("intrinsic constraint %1 check = %2").arg(s).arg(d);
354+
qDebug().noquote() << red << QString("intrinsic constraint %1 check = %2").arg(s).arg(d) << black;
336355
}
337356
}
338357
#endif
339358

340359
// check all(!) geometric constraints..........................
341-
constexpr int width = 25;
360+
//constexpr int width = 25;
342361
for ( int c=0; c<C; c++ )
343362
{
344363
auto & con = constr->at( mapc(c) );
@@ -354,14 +373,23 @@ void AdjustmentFramework::a_check_constraints(
354373
d = con->contradict( idx, l0() ).norm();
355374
con->setEnforced( fabs(d) < threshold_numericalCheck() );
356375

376+
#ifdef QT_DEBUG
357377
if ( verbose ) {
358-
QString msg = QString("%1: check = %2").arg(
359-
QString::fromLatin1( con->type_name()), width).arg(d);
360-
361-
qDebug().noquote() << (con->enforced() ? green : red)
362-
<< msg << black;
378+
QString msg1 = QString("%1: ").arg(
379+
QString::fromLatin1( con->type_name()), 12);
380+
QString msg2 = QString("check = %2, \t").arg(d);
381+
QDebug deb = qDebug().noquote();
382+
deb << QString("constraint #%1: ").arg(c+1,3);
383+
deb << (con->required() ? green : blue) << msg1 << black;
384+
deb << (con->enforced() ? black : red) << msg2 << black;
385+
386+
auto idxx = bi->findInColumn( mapc(c) );
387+
for ( Index i=0; i<idxx.size(); i++ ) {
388+
int idxxx = indexOf( maps, idxx(i) );
389+
deb << idxxx+1;
390+
}
363391
}
364-
392+
#endif
365393
}
366394
}
367395

‎src/adjustment.h‎

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by
@@ -59,7 +59,7 @@ class AdjustmentFramework
5959
AdjustmentFramework & operator= ( AdjustmentFramework &&) = delete;
6060

6161
//! Enforce the constraints of a subtask (adjustment)
62-
bool enforce_constraints(const QVector<std::shared_ptr<ConstraintBase> > *constr,
62+
bool enforce_constraints( const QVector<std::shared_ptr<ConstraintBase> > *constr,
6363
const Graph::IncidenceMatrix *Bi,
6464
const RowVectorXi &maps,
6565
const RowVectorXi &mapc);
@@ -84,19 +84,19 @@ class AdjustmentFramework
8484
void reset() { l0_ = l_;}
8585
void update( Index start, const VectorXd &x );
8686

87-
void a_Jacobian( const QVector<std::shared_ptr<ConstraintBase> > *constr,
88-
const Graph::IncidenceMatrix *Bi,
89-
SparseMatrix<double, Eigen::ColMajor> & BBr,
90-
VectorXd & g0,
91-
const RowVectorXi & maps,
92-
const RowVectorXi & mapc) const;
87+
void Jacobian( const QVector<std::shared_ptr<ConstraintBase> > *constr,
88+
const Graph::IncidenceMatrix *Bi,
89+
SparseMatrix<double, Eigen::ColMajor> & BBr,
90+
VectorXd & g0,
91+
const RowVectorXi & maps,
92+
const RowVectorXi & mapc) const;
9393

94-
void a_check_constraints( const QVector<std::shared_ptr<ConstraintBase> > *constr,
95-
const Graph::IncidenceMatrix *bi,
96-
const RowVectorXi & maps,
97-
const RowVectorXi & mapc ) const;
94+
void check_constraints( const QVector<std::shared_ptr<ConstraintBase> > *constr,
95+
const Graph::IncidenceMatrix *bi,
96+
const RowVectorXi & maps,
97+
const RowVectorXi & mapc ) const;
9898

99-
bool verbose = false;
99+
bool verbose = true;
100100
VectorXd l0_; // vector of approximate/estimated observations
101101
const VectorXd l_; // vector of observations.
102102
const MatrixXd Cov_ll_; // covariance matrix of vector of observations
@@ -110,8 +110,10 @@ class AdjustmentFramework
110110
const double numericalCheck = 1e-5; // numerical check constraints
111111
} threshold_;
112112

113+
//! Rotation matrix for minimal rotation
113114
MatrixXd Rot_ab( const VectorXd & a,
114115
const VectorXd & b) const;
116+
//! Nullspace of row vector
115117
MatrixXd null( const VectorXd & xs ) const;
116118

117119
Index indexOf( const Eigen::VectorXi & v, int x) const;

‎src/commands.cpp‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by
@@ -31,7 +31,7 @@ GUI::MainScene * Cmd::Undo::s_scene = nullptr;
3131

3232
Undo::Undo( QUndoCommand *parent) : QUndoCommand(parent)
3333
{
34-
qDebug() << Q_FUNC_INFO;
34+
// qDebug() << Q_FUNC_INFO;
3535
}
3636

3737

@@ -40,7 +40,7 @@ AddStroke::AddStroke( State *curr,
4040
std::unique_ptr<State> &n,
4141
QUndoCommand *parent) : Undo(parent)
4242
{
43-
qDebug() << Q_FUNC_INFO;
43+
// qDebug() << Q_FUNC_INFO;
4444
setText( "add stroke" );
4545
current_state_ = curr; // set pointer
4646
prev_state_ = std::move(p);
@@ -49,15 +49,15 @@ AddStroke::AddStroke( State *curr,
4949

5050
void AddStroke::redo()
5151
{
52-
qDebug() << Q_FUNC_INFO;
52+
// qDebug() << Q_FUNC_INFO;
5353
*current_state_ = *next_state_; // copy content
5454
scene()->removeAllItems();
5555
scene()->addGraphicItems( next_state_.get() );
5656
}
5757

5858
void AddStroke::undo()
5959
{
60-
qDebug() << Q_FUNC_INFO;
60+
// qDebug() << Q_FUNC_INFO;
6161
*current_state_ = *prev_state_; // set
6262
scene()->removeAllItems();
6363
scene()->addGraphicItems( prev_state_.get() );
@@ -94,7 +94,7 @@ void DeleteSelection::undo()
9494

9595
TabulaRasa::TabulaRasa( State *st, QUndoCommand *parent ) : Undo(parent)
9696
{
97-
qDebug() << Q_FUNC_INFO ;
97+
// qDebug() << Q_FUNC_INFO ;
9898
setText( "clear all" );
9999
current_state_ = st; // pointer
100100
prev_state_ = std::make_unique<State>(*st); // copy
@@ -106,14 +106,14 @@ TabulaRasa::TabulaRasa( State *st, QUndoCommand *parent ) : Undo(parent)
106106

107107
void TabulaRasa::redo()
108108
{
109-
qDebug() << Q_FUNC_INFO;
109+
// qDebug() << Q_FUNC_INFO;
110110
*current_state_ = *next_state_;
111111
scene()->removeAllItems();
112112
}
113113

114114
void TabulaRasa::undo()
115115
{
116-
qDebug() << Q_FUNC_INFO;
116+
// qDebug() << Q_FUNC_INFO;
117117
*current_state_ = *prev_state_;
118118
scene()->addGraphicItems( prev_state_.get() );
119119
}
@@ -125,7 +125,7 @@ ReplaceStateWithFileContent::ReplaceStateWithFileContent( const QString & fileNa
125125
QUndoCommand *parent)
126126
: Undo(parent)
127127
{
128-
qDebug() << Q_FUNC_INFO;
128+
// qDebug() << Q_FUNC_INFO;
129129
setText( fileName );
130130

131131
current_state_ = curr; // pointer
@@ -136,15 +136,15 @@ ReplaceStateWithFileContent::ReplaceStateWithFileContent( const QString & fileNa
136136

137137
void ReplaceStateWithFileContent::redo()
138138
{
139-
qDebug() << Q_FUNC_INFO;
139+
// qDebug() << Q_FUNC_INFO;
140140
*current_state_ = *next_state_;
141141
scene()->removeAllItems();
142142
scene()->addGraphicItems( next_state_.get() );
143143
}
144144

145145
void ReplaceStateWithFileContent::undo()
146146
{
147-
qDebug() << Q_FUNC_INFO;
147+
// qDebug() << Q_FUNC_INFO;
148148
*current_state_ = *prev_state_;
149149
scene()->removeAllItems();
150150
scene()->addGraphicItems( prev_state_.get() );

‎src/commands.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by

‎src/conics.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by

‎src/conics.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/*
22
* This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
3-
* Copyright (c) 2022 Jochen Meidow, Fraunhofer IOSB
3+
* Copyright (c) 2022-2023 Jochen Meidow, Fraunhofer IOSB
44
*
55
* This program is free software: you can redistribute it and/or modify
66
* it under the terms of the GNU General Public License as published by

0 commit comments

Comments
 (0)