Point Cloud Library (PCL)
1.10.1
pcl
surface
3rdparty
opennurbs
opennurbs_matrix.h
1
/* $NoKeywords: $ */
2
/*
3
//
4
// Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
5
// OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
6
// McNeel & Associates.
7
//
8
// THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
9
// ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
10
// MERCHANTABILITY ARE HEREBY DISCLAIMED.
11
//
12
// For complete openNURBS copyright information see <http://www.opennurbs.org>.
13
//
14
////////////////////////////////////////////////////////////////
15
*/
16
17
#if !defined(ON_MATRIX_INC_)
18
#define ON_MATRIX_INC_
19
20
class
ON_Xform
;
21
22
class
ON_CLASS
ON_Matrix
23
{
24
public
:
25
ON_Matrix
();
26
ON_Matrix
(
27
int
row_count,
28
int
col_count
29
);
30
ON_Matrix
(
// see ON_Matrix::Create(int,int,int,int) for details
31
int
,
// first valid row index
32
int
,
// last valid row index
33
int
,
// first valid column index
34
int
// last valid column index
35
);
36
ON_Matrix
(
const
ON_Xform
& );
37
ON_Matrix
(
const
ON_Matrix
& );
38
39
/*
40
Description:
41
This constructor is for experts who have storage for a matrix
42
and need to use it in ON_Matrix form.
43
Parameters:
44
row_count - [in]
45
col_count - [in]
46
M - [in]
47
bDestructorFreeM - [in]
48
If true, ~ON_Matrix will call onfree(M).
49
If false, caller is managing M's memory.
50
Remarks:
51
ON_Matrix functions that increase the value of row_count or col_count
52
will fail on a matrix created with this constructor.
53
*/
54
ON_Matrix
(
55
int
row_count,
56
int
col_count,
57
double
** M,
58
bool
bDestructorFreeM
59
);
60
61
virtual
~
ON_Matrix
();
62
void
EmergencyDestroy();
// call if memory pool used matrix by becomes invalid
63
64
// ON_Matrix[i][j] = value at row i and column j
65
// 0 <= i < RowCount()
66
// 0 <= j < ColCount()
67
double
* operator[](
int
);
68
const
double
* operator[](
int
)
const
;
69
70
ON_Matrix
& operator=(
const
ON_Matrix
&);
71
ON_Matrix
& operator=(
const
ON_Xform
&);
72
73
bool
IsValid()
const
;
74
int
IsSquare()
const
;
// returns 0 for no and m_row_count (= m_col_count) for yes
75
int
RowCount()
const
;
76
int
ColCount()
const
;
77
int
MinCount()
const
;
// smallest of row and column count
78
int
MaxCount()
const
;
// largest of row and column count
79
80
void
RowScale(
int
,
double
);
81
void
ColScale(
int
,
double
);
82
void
RowOp(
int
,
double
,
int
);
83
void
ColOp(
int
,
double
,
int
);
84
85
bool
Create(
86
int
,
// number of rows
87
int
// number of columns
88
);
89
90
bool
Create(
// E.g., Create(1,5,1,7) creates a 5x7 sized matrix that with
91
// "top" row = m[1][1],...,m[1][7] and "bottom" row
92
// = m[5][1],...,m[5][7]. The result of Create(0,m,0,n) is
93
// identical to the result of Create(m+1,n+1).
94
int
,
// first valid row index
95
int
,
// last valid row index
96
int
,
// first valid column index
97
int
// last valid column index
98
);
99
100
/*
101
Description:
102
This constructor is for experts who have storage for a matrix
103
and need to use it in ON_Matrix form.
104
Parameters:
105
row_count - [in]
106
col_count - [in]
107
M - [in]
108
bDestructorFreeM - [in]
109
If true, ~ON_Matrix will call onfree(M).
110
If false, caller is managing M's memory.
111
Remarks:
112
ON_Matrix functions that increase the value of row_count or col_count
113
will fail on a matrix created with this constructor.
114
*/
115
bool
Create(
116
int
row_count,
117
int
col_count,
118
double
** M,
119
bool
bDestructorFreeM
120
);
121
122
123
void
Destroy();
124
125
void
Zero();
126
127
void
SetDiagonal(
double
);
// sets diagonal value and zeros off diagonal values
128
void
SetDiagonal(
const
double
*);
// sets diagonal values and zeros off diagonal values
129
void
SetDiagonal(
int
,
const
double
*);
// sets size to count x count and diagonal values and zeros off diagonal values
130
void
SetDiagonal(
const
ON_SimpleArray<double>
&);
// sets size to length X lengthdiagonal values and zeros off diagonal values
131
132
bool
Transpose();
133
134
bool
SwapRows(
int
,
int
);
// ints are row indices to swap
135
bool
SwapCols(
int
,
int
);
// ints are col indices to swap
136
bool
Invert(
137
double
// zero tolerance
138
);
139
140
/*
141
Description:
142
Set this = A*B.
143
Parameters:
144
A - [in]
145
(Can be this)
146
B - [in]
147
(Can be this)
148
Returns:
149
True when A is an mXk matrix and B is a k X n matrix; in which case
150
"this" will be an mXn matrix = A*B.
151
False when A.ColCount() != B.RowCount().
152
*/
153
bool
Multiply(
const
ON_Matrix
& A,
const
ON_Matrix
&
B
);
154
155
/*
156
Description:
157
Set this = A+B.
158
Parameters:
159
A - [in]
160
(Can be this)
161
B - [in]
162
(Can be this)
163
Returns:
164
True when A and B are mXn matrices; in which case
165
"this" will be an mXn matrix = A+B.
166
False when A and B have different sizes.
167
*/
168
bool
Add(
const
ON_Matrix
& A,
const
ON_Matrix
&
B
);
169
170
171
/*
172
Description:
173
Set this = s*this.
174
Parameters:
175
s - [in]
176
Returns:
177
True when A and s are valid.
178
*/
179
bool
Scale(
double
s );
180
181
182
// Description:
183
// Row reduce a matrix to calculate rank and determinant.
184
// Parameters:
185
// zero_tolerance - [in] (>=0.0) zero tolerance for pivot test
186
// If the absolute value of a pivot is <= zero_tolerance,
187
// then the pivot is assumed to be zero.
188
// determinant - [out] value of determinant is returned here.
189
// pivot - [out] value of the smallest pivot is returned here
190
// Returns:
191
// Rank of the matrix.
192
// Remarks:
193
// The matrix itself is row reduced so that the result is
194
// an upper triangular matrix with 1's on the diagonal.
195
int
RowReduce(
// returns rank
196
double
,
// zero_tolerance
197
double
&,
// determinant
198
double
&
// pivot
199
);
200
201
// Description:
202
// Row reduce a matrix as the first step in solving M*X=B where
203
// B is a column of values.
204
// Parameters:
205
// zero_tolerance - [in] (>=0.0) zero tolerance for pivot test
206
// If the absolute value of a pivot is <= zero_tolerance,
207
// then the pivot is assumed to be zero.
208
// B - [in/out] an array of m_row_count values that is row reduced
209
// with the matrix.
210
// determinant - [out] value of determinant is returned here.
211
// pivot - [out] If not NULL, then the value of the smallest
212
// pivot is returned here
213
// Returns:
214
// Rank of the matrix.
215
// Remarks:
216
// The matrix itself is row reduced so that the result is
217
// an upper triangular matrix with 1's on the diagonal.
218
// Example:
219
// Solve M*X=B;
220
// double B[m] = ...;
221
// double B[n] = ...;
222
// ON_Matrix M(m,n) = ...;
223
// M.RowReduce(ON_ZERO_TOLERANCE,B); // modifies M and B
224
// M.BackSolve(m,B,X); // solution is in X
225
// See Also:
226
// ON_Matrix::BackSolve
227
int
RowReduce(
228
double
,
// zero_tolerance
229
double
*,
// B
230
double
* = NULL
// pivot
231
);
232
233
// Description:
234
// Row reduce a matrix as the first step in solving M*X=B where
235
// B is a column of 3d points
236
// Parameters:
237
// zero_tolerance - [in] (>=0.0) zero tolerance for pivot test
238
// If the absolute value of a pivot is <= zero_tolerance,
239
// then the pivot is assumed to be zero.
240
// B - [in/out] an array of m_row_count 3d points that is
241
// row reduced with the matrix.
242
// determinant - [out] value of determinant is returned here.
243
// pivot - [out] If not NULL, then the value of the smallest
244
// pivot is returned here
245
// Returns:
246
// Rank of the matrix.
247
// Remarks:
248
// The matrix itself is row reduced so that the result is
249
// an upper triangular matrix with 1's on the diagonal.
250
// See Also:
251
// ON_Matrix::BackSolve
252
int
RowReduce(
253
double
,
// zero_tolerance
254
ON_3dPoint
*,
// B
255
double
* = NULL
// pivot
256
);
257
258
// Description:
259
// Row reduce a matrix as the first step in solving M*X=B where
260
// B is a column arbitrary dimension points.
261
// Parameters:
262
// zero_tolerance - [in] (>=0.0) zero tolerance for pivot test
263
// If a the absolute value of a pivot is <= zero_tolerance,
264
// then the pivoit is assumed to be zero.
265
// pt_dim - [in] dimension of points
266
// pt_stride - [in] stride between points (>=pt_dim)
267
// pt - [in/out] array of m_row_count*pt_stride values.
268
// The i-th point is
269
// (pt[i*pt_stride],...,pt[i*pt_stride+pt_dim-1]).
270
// This array of points is row reduced along with the
271
// matrix.
272
// pivot - [out] If not NULL, then the value of the smallest
273
// pivot is returned here
274
// Returns:
275
// Rank of the matrix.
276
// Remarks:
277
// The matrix itself is row reduced so that the result is
278
// an upper triangular matrix with 1's on the diagonal.
279
// See Also:
280
// ON_Matrix::BackSolve
281
int
RowReduce(
// returns rank
282
double
,
// zero_tolerance
283
int
,
// pt_dim
284
int
,
// pt_stride
285
double
*,
// pt
286
double
* = NULL
// pivot
287
);
288
289
// Description:
290
// Solve M*X=B where M is upper triangular with a unit diagonal and
291
// B is a column of values.
292
// Parameters:
293
// zero_tolerance - [in] (>=0.0) used to test for "zero" values in B
294
// in under determined systems of equations.
295
// Bsize - [in] (>=m_row_count) length of B. The values in
296
// B[m_row_count],...,B[Bsize-1] are tested to make sure they are
297
// "zero".
298
// B - [in] array of length Bsize.
299
// X - [out] array of length m_col_count. Solutions returned here.
300
// Remarks:
301
// Actual values M[i][j] with i <= j are ignored.
302
// M[i][i] is assumed to be one and M[i][j] i<j is assumed to be zero.
303
// For square M, B and X can point to the same memory.
304
// See Also:
305
// ON_Matrix::RowReduce
306
bool
BackSolve(
307
double
,
// zero_tolerance
308
int
,
// Bsize
309
const
double
*,
// B
310
double
*
// X
311
)
const
;
312
313
// Description:
314
// Solve M*X=B where M is upper triangular with a unit diagonal and
315
// B is a column of 3d points.
316
// Parameters:
317
// zero_tolerance - [in] (>=0.0) used to test for "zero" values in B
318
// in under determined systems of equations.
319
// Bsize - [in] (>=m_row_count) length of B. The values in
320
// B[m_row_count],...,B[Bsize-1] are tested to make sure they are
321
// "zero".
322
// B - [in] array of length Bsize.
323
// X - [out] array of length m_col_count. Solutions returned here.
324
// Remarks:
325
// Actual values M[i][j] with i <= j are ignored.
326
// M[i][i] is assumed to be one and M[i][j] i<j is assumed to be zero.
327
// For square M, B and X can point to the same memory.
328
// See Also:
329
// ON_Matrix::RowReduce
330
bool
BackSolve(
331
double
,
// zero_tolerance
332
int
,
// Bsize
333
const
ON_3dPoint
*,
// B
334
ON_3dPoint
*
// X
335
)
const
;
336
337
// Description:
338
// Solve M*X=B where M is upper triangular with a unit diagonal and
339
// B is a column of points
340
// Parameters:
341
// zero_tolerance - [in] (>=0.0) used to test for "zero" values in B
342
// in under determined systems of equations.
343
// pt_dim - [in] dimension of points
344
// Bsize - [in] (>=m_row_count) number of points in B[]. The points
345
// correspoinding to indices m_row_count, ..., (Bsize-1)
346
// are tested to make sure they are "zero".
347
// Bpt_stride - [in] stride between B points (>=pt_dim)
348
// Bpt - [in/out] array of m_row_count*Bpt_stride values.
349
// The i-th B point is
350
// (Bpt[i*Bpt_stride],...,Bpt[i*Bpt_stride+pt_dim-1]).
351
// Xpt_stride - [in] stride between X points (>=pt_dim)
352
// Xpt - [out] array of m_col_count*Xpt_stride values.
353
// The i-th X point is
354
// (Xpt[i*Xpt_stride],...,Xpt[i*Xpt_stride+pt_dim-1]).
355
// Remarks:
356
// Actual values M[i][j] with i <= j are ignored.
357
// M[i][i] is assumed to be one and M[i][j] i<j is assumed to be zero.
358
// For square M, B and X can point to the same memory.
359
// See Also:
360
// ON_Matrix::RowReduce
361
bool
BackSolve(
362
double
,
// zero_tolerance
363
int
,
// pt_dim
364
int
,
// Bsize
365
int
,
// Bpt_stride
366
const
double
*,
// Bpt
367
int
,
// Xpt_stride
368
double
*
// Xpt
369
)
const
;
370
371
bool
IsRowOrthoganal()
const
;
372
bool
IsRowOrthoNormal()
const
;
373
374
bool
IsColOrthoganal()
const
;
375
bool
IsColOrthoNormal()
const
;
376
377
378
double
**
m
;
// m[i][j] = value at row i and column j
379
// 0 <= i < RowCount()
380
// 0 <= j < ColCount()
381
private
:
382
int
m_row_count;
383
int
m_col_count;
384
// m_rowmem[i][j] = row i+m_row_offset and column j+m_col_offset.
385
ON_SimpleArray<double*>
m_rowmem;
386
double
** m_Mmem;
// used by Create(row_count,col_count,user_memory,true);
387
int
m_row_offset;
// = ri0 when sub-matrix constructor is used
388
int
m_col_offset;
// = ci0 when sub-matrix constructor is used
389
void
* m_cmem;
390
// returns 0 based arrays, even in submatrix case.
391
double
const
*
const
* ThisM()
const
;
392
double
* * ThisM();
393
};
394
395
/*
396
Description:
397
Calculate the singular value decomposition of a matrix.
398
399
Parameters:
400
row_count - [in]
401
number of rows in matrix A
402
col_count - [in]
403
number of columns in matrix A
404
A - [in]
405
Matrix for which you want the singular value decomposition.
406
A[0][0] = coefficeint in the first row and first column.
407
A[row_count-1][col_count-1] = coefficeint in the last row
408
and last column.
409
U - [out]
410
The singular value decomposition of A is U*Diag(W)*Transpose(V),
411
where U has the same size as A, Diag(W) is a col_count X col_count
412
diagonal matrix with (W[0],...,W[col_count-1]) on the diagonal
413
and V is a col_count X col_count matrix.
414
U and A may be the same pointer. If the input value of U is
415
null, heap storage will be allocated using onmalloc()
416
and the calling function must call onfree(U). If the input
417
value of U is not null, U[i] must point to an array of col_count
418
doubles.
419
W - [out]
420
If the input value W is null, then heap storage will be allocated
421
using onmalloc() and the calling function must call onfree(W).
422
If the input value of W is not null, then W must point to
423
an array of col_count doubles.
424
V - [out]
425
If the input value V is null, then heap storage will be allocated
426
using onmalloc() and the calling function must call onfree(V).
427
If the input value of V is not null, then V[i] must point
428
to an array of col_count doubles.
429
430
Example:
431
432
int m = row_count;
433
int n = col_count;
434
ON_Matrix A(m,n);
435
for (i = 0; i < m; i++ ) for ( j = 0; j < n; j++ )
436
{
437
A[i][j] = ...;
438
}
439
ON_Matrix U(m,n);
440
double* W = 0; // ON_GetMatrixSVD() will allocate W
441
ON_Matrix V(n,n);
442
bool rc = ON_GetMatrixSVD(m,n,A.m,U.m,W,V.m);
443
...
444
onfree(W); // W allocated in ON_GetMatrixSVD()
445
446
Returns:
447
True if the singular value decomposition was cacluated.
448
False if the algorithm failed to converge.
449
*/
450
ON_DECL
451
bool
ON_GetMatrixSVD(
452
int
row_count,
453
int
col_count,
454
double
const
*
const
* A,
455
double
**& U,
456
double
*& W,
457
double
**& V
458
);
459
460
/*
461
Description:
462
Invert the diagonal matrix in a the singular value decomposition.
463
Parameters:
464
count - [in] number of elements in W
465
W - [in]
466
diagonal values in the singular value decomposition.
467
invW - [out]
468
The inverted diagonal is returned here. invW may be the same
469
pointer as W. If the input value of invW is not null, it must
470
point to an array of count doubles. If the input value of
471
invW is null, heap storage will be allocated using onmalloc()
472
and the calling function must call onfree(invW).
473
Remarks:
474
If the singular value decomposition were mathematically perfect, then
475
this function would be:
476
for (i = 0; i < count; i++)
477
invW[i] = (W[i] != 0.0) ? 1.0/W[i] : 0.0;
478
Because the double precision arithmetic is not mathematically perfect,
479
very small values of W[i] may well be zero and this function makes
480
a reasonable guess as to when W[i] should be treated as zero.
481
Returns:
482
Number of non-zero elements in invW, which, in a mathematically perfect
483
situation, is the rank of Diag(W).
484
*/
485
ON_DECL
486
int
ON_InvertSVDW(
487
int
count,
488
const
double
* W,
489
double
*& invW
490
);
491
492
/*
493
Description:
494
Solve a linear system of equations using the singular value decomposition.
495
Parameters:
496
row_count - [in]
497
number of rows in matrix U
498
col_count - [in]
499
number of columns in matrix U
500
U - [in]
501
row_count X col_count matix.
502
See the remarks section for the definition of U.
503
invW - [in]
504
inverted DVD diagonal.
505
See the remarks section for the definition of invW.
506
V - [in]
507
col_count X col_count matrix.
508
See the remarks section for the definition of V.
509
B - [in]
510
An array of row_count values.
511
X - [out]
512
The solution array of col_count values is returned here.
513
If the input value of X is not null, it must point to an
514
array of col_count doubles. If the input value of X is
515
null, heap storage will be allocated using onmalloc() and
516
the calling function must call onfree(X).
517
Remarks:
518
If A*X = B is an m X n system of equations (m = row_count, n = col_count)
519
and A = U*Diag(W)*Transpose(V) is the singular value decompostion of A,
520
then a solution is X = V*Diag(1/W)*Transpose(U).
521
Example:
522
523
int m = row_count;
524
int n = col_count;
525
ON_Matrix A(m,n);
526
for (i = 0; i < m; i++ ) for ( j = 0; j < n; j++ )
527
{
528
A[i][j] = ...;
529
}
530
ON_SimpleArray<double> B(m);
531
for (i = 0; i < m; i++ )
532
{
533
B[i] = ...;
534
}
535
536
ON_SimpleArray<double> X; // solution returned here.
537
{
538
double** U = 0;
539
double* W = 0;
540
double** V = 0;
541
if ( ON_GetMatrixSVD(m,n,A.m,U,W,V) )
542
{
543
double* invW = 0;
544
int rankW = ON_InvertSVDW(n,W,W); // save invW into W
545
X.Reserve(n);
546
if ( ON_SolveSVD(m,n,U,W,V,B,X.Array()) )
547
X.SetCount(n);
548
}
549
onfree(U); // U allocated in ON_GetMatrixSVD()
550
onfree(W); // W allocated in ON_GetMatrixSVD()
551
onfree(V); // V allocated in ON_GetMatrixSVD()
552
}
553
554
if ( n == X.Count() )
555
{
556
... use solution
557
}
558
Returns:
559
True if input is valid and X[] was calculated.
560
False if input is not valid.
561
*/
562
ON_DECL
563
bool
ON_SolveSVD(
564
int
row_count,
565
int
col_count,
566
double
const
*
const
* U,
567
const
double
* invW,
568
double
const
*
const
* V,
569
const
double
*
B
,
570
double
*& X
571
);
572
573
574
/*
575
Description:
576
Perform simple row reduction on a matrix. If A is square, positive
577
definite, and really really nice, then the returned B is the inverse
578
of A. If A is not positive definite and really really nice, then it
579
is probably a waste of time to call this function.
580
Parameters:
581
row_count - [in]
582
col_count - [in]
583
zero_pivot - [in]
584
absolute values <= zero_pivot are considered to be zero
585
A - [in/out]
586
A row_count X col_count matrix. Input is the matrix to be
587
row reduced. The calculation destroys A, so output A is garbage.
588
B - [out]
589
A a row_count X row_count matrix. That records the row reduction.
590
pivots - [out]
591
minimum and maximum absolute values of pivots.
592
Returns:
593
Rank of A. If the returned value < min(row_count,col_count),
594
then a zero pivot was encountered.
595
If C = input value of A, then B*C = (I,*)
596
*/
597
ON_DECL
598
int
ON_RowReduce(
599
int
row_count,
600
int
col_count,
601
double
zero_pivot,
602
double
** A,
603
double
**
B
,
604
double
pivots[2]
605
);
606
607
#endif
ON_Matrix::m
double ** m
Definition:
opennurbs_matrix.h:378
ON_Xform
Definition:
opennurbs_xform.h:28
ON_SimpleArray< double >
ON_Matrix
Definition:
opennurbs_matrix.h:22
ON_3dPoint
Definition:
opennurbs_point.h:418
pcl::B
Definition:
norms.h:54