source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/eigen/test/umeyama.cpp

Last change on this file was 136, checked in by ldecherf, 8 years ago

Doc

File size: 5.7 KB
Line 
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10#include "main.h"
11
12#include <Eigen/Core>
13#include <Eigen/Geometry>
14
15#include <Eigen/LU> // required for MatrixBase::determinant
16#include <Eigen/SVD> // required for SVD
17
18using namespace Eigen;
19
20// Constructs a random matrix from the unitary group U(size).
21template <typename T>
22Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
23{
24 typedef T Scalar;
25 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
26
27 MatrixType Q;
28
29 int max_tries = 40;
30 double is_unitary = false;
31
32 while (!is_unitary && max_tries > 0)
33 {
34 // initialize random matrix
35 Q = MatrixType::Random(size, size);
36
37 // orthogonalize columns using the Gram-Schmidt algorithm
38 for (int col = 0; col < size; ++col)
39 {
40 typename MatrixType::ColXpr colVec = Q.col(col);
41 for (int prevCol = 0; prevCol < col; ++prevCol)
42 {
43 typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
44 colVec -= colVec.dot(prevColVec)*prevColVec;
45 }
46 Q.col(col) = colVec.normalized();
47 }
48
49 // this additional orthogonalization is not necessary in theory but should enhance
50 // the numerical orthogonality of the matrix
51 for (int row = 0; row < size; ++row)
52 {
53 typename MatrixType::RowXpr rowVec = Q.row(row);
54 for (int prevRow = 0; prevRow < row; ++prevRow)
55 {
56 typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
57 rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
58 }
59 Q.row(row) = rowVec.normalized();
60 }
61
62 // final check
63 is_unitary = Q.isUnitary();
64 --max_tries;
65 }
66
67 if (max_tries == 0)
68 eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");
69
70 return Q;
71}
72
73// Constructs a random matrix from the special unitary group SU(size).
74template <typename T>
75Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
76{
77 typedef T Scalar;
78
79 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
80
81 // initialize unitary matrix
82 MatrixType Q = randMatrixUnitary<Scalar>(size);
83
84 // tweak the first column to make the determinant be 1
85 Q.col(0) *= numext::conj(Q.determinant());
86
87 return Q;
88}
89
90template <typename MatrixType>
91void run_test(int dim, int num_elements)
92{
93 using std::abs;
94 typedef typename internal::traits<MatrixType>::Scalar Scalar;
95 typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
96 typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
97
98 // MUST be positive because in any other case det(cR_t) may become negative for
99 // odd dimensions!
100 const Scalar c = abs(internal::random<Scalar>());
101
102 MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
103 VectorX t = Scalar(50)*VectorX::Random(dim,1);
104
105 MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
106 cR_t.block(0,0,dim,dim) = c*R;
107 cR_t.block(0,dim,dim,1) = t;
108
109 MatrixX src = MatrixX::Random(dim+1, num_elements);
110 src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
111
112 MatrixX dst = cR_t*src;
113
114 MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));
115
116 const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
117 VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
118}
119
120template<typename Scalar, int Dimension>
121void run_fixed_size_test(int num_elements)
122{
123 using std::abs;
124 typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
125 typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
126 typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
127 typedef Matrix<Scalar, Dimension, 1> FixedVector;
128
129 const int dim = Dimension;
130
131 // MUST be positive because in any other case det(cR_t) may become negative for
132 // odd dimensions!
133 // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744)
134 const Scalar c = internal::random<Scalar>(0.5, 2.0);
135
136 FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
137 FixedVector t = Scalar(32)*FixedVector::Random(dim,1);
138
139 HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1);
140 cR_t.block(0,0,dim,dim) = c*R;
141 cR_t.block(0,dim,dim,1) = t;
142
143 MatrixX src = MatrixX::Random(dim+1, num_elements);
144 src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
145
146 MatrixX dst = cR_t*src;
147
148 Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements);
149 Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements);
150
151 HomMatrix cR_t_umeyama = umeyama(src_block, dst_block);
152
153 const Scalar error = ( cR_t_umeyama*src - dst ).squaredNorm();
154
155 VERIFY(error < Scalar(16)*std::numeric_limits<Scalar>::epsilon());
156}
157
158void test_umeyama()
159{
160 for (int i=0; i<g_repeat; ++i)
161 {
162 const int num_elements = internal::random<int>(40,500);
163
164 // works also for dimensions bigger than 3...
165 for (int dim=2; dim<8; ++dim)
166 {
167 CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements));
168 CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements));
169 }
170
171 CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements)));
172 CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements)));
173 CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements)));
174
175 CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements)));
176 CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements)));
177 CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements)));
178 }
179
180 // Those two calls don't compile and result in meaningful error messages!
181 // umeyama(MatrixXcf(),MatrixXcf());
182 // umeyama(MatrixXcd(),MatrixXcd());
183}
Note: See TracBrowser for help on using the repository browser.