-
计算旋转矩阵的逆矩阵，应用SVD分解法
1 #pragma warning(disable:4996)
2 #include <pcl/registration/ia_ransac.h>//采样一致性
3 #include <pcl/point_types.h>
4 #include <pcl/point_cloud.h>
5 #include <pcl/features/normal_3d.h>
6 #include <pcl/features/fpfh.h>
7 #include <pcl/search/kdtree.h>
8 #include <pcl/io/pcd_io.h>
9 #include <pcl/filters/voxel_grid.h>//
10 #include <pcl/filters/filter.h>//
11 #include <pcl/registration/icp.h>//icp配准
12 #include <pcl/visualization/pcl_visualizer.h>//可视化
13 #include <time.h>//时间
14 #include <math.h>
15 #include <vector>
16 using namespace std;
17 using pcl::NormalEstimation;
18 using pcl::search::KdTree;
19 typedef pcl::PointXYZ PointT;
20 typedef pcl::PointCloud<PointT> PointCloud;
21 int l,k;
22 float a, b;
23 double x, y, z, c, s;
24 #define N 4
25 //LUP分解
26 void LUP_Descomposition(double A[N*N], double L[N*N], double U[N*N], int P[N])
27 {
28 int row = 0;
29 for (int i = 0; i < N; i++)
30 {
31 P[i] = i;
32 }
33 for (int i = 0; i < N - 1; i++)
34 {
35 double p = 0.0;
36 for (int j = i; j < N; j++)
37 {
38 if (abs(A[j*N + i]) > p)
39 {
40 p = abs(A[j*N + i]);
41 row = j;
42 }
43 }
44 if (0 == p)
45 {
46 cout << "矩阵奇异，无法计算逆" << endl;
47 return;
48 }
49
50 //交换P[i]和P[row]
51 int tmp = P[i];
52 P[i] = P[row];
53 P[row] = tmp;
54
55 double tmp2 = 0.0;
56 for (int j = 0; j < N; j++)
57 {
58 //交换A[i][j]和 A[row][j]
59 tmp2 = A[i*N + j];
60 A[i*N + j] = A[row*N + j];
61 A[row*N + j] = tmp2;
62 }
63
64 //以下同LU分解
65 double u = A[i*N + i], l = 0.0;
66 for (int j = i + 1; j < N; j++)
67 {
68 l = A[j*N + i] / u;
69 A[j*N + i] = l;
70 for (int k = i + 1; k < N; k++)
71 {
72 A[j*N + k] = A[j*N + k] - A[i*N + k] * l;
73 }
74 }
75
76 }
77
78 //构造L和U
79 for (int i = 0; i < N; i++)
80 {
81 for (int j = 0; j <= i; j++)
82 {
83 if (i != j)
84 {
85 L[i*N + j] = A[i*N + j];
86 }
87 else
88 {
89 L[i*N + j] = 1;
90 }
91 }
92 for (int k = i; k < N; k++)
93 {
94 U[i*N + k] = A[i*N + k];
95 }
96 }
97
98 }
99
100 //LUP求解方程
101 double * LUP_Solve(double L[N*N], double U[N*N], int P[N], double b[N])
102 {
103 double *x = new double[N]();
104 double *y = new double[N]();
105
106 //正向替换
107 for (int i = 0; i < N; i++)
108 {
109 y[i] = b[P[i]];
110 for (int j = 0; j < i; j++)
111 {
112 y[i] = y[i] - L[i*N + j] * y[j];
113 }
114 }
115 //反向替换
116 for (int i = N - 1; i >= 0; i--)
117 {
118 x[i] = y[i];
119 for (int j = N - 1; j > i; j--)
120 {
121 x[i] = x[i] - U[i*N + j] * x[j];
122 }
123 x[i] /= U[i*N + i];
124 }
125 return x;
126 }
127 /*****************矩阵原地转置BEGIN********************/
128 /* 后继 */
129 int getNext(int i, int m, int n)
130 {
131 return (i%n)*m + i / n;
132 }
133
134 /* 前驱 */
135 int getPre(int i, int m, int n)
136 {
137 return (i%m)*n + i / m;
138 }
139
140 /* 处理以下标i为起点的环 */
141 void movedata(double *mtx, int i, int m, int n)
142 {
143 double temp = mtx[i]; // 暂存
144 int cur = i; // 当前下标
145 int pre = getPre(cur, m, n);
146 while (pre != i)
147 {
148 mtx[cur] = mtx[pre];
149 cur = pre;
150 pre = getPre(cur, m, n);
151 }
152 mtx[cur] = temp;
153 }
154
155 /* 转置，即循环处理所有环 */
156 void transpose(double *mtx, int m, int n)
157 {
158 for (int i = 0; i < m*n; ++i)
159 {
160 int next = getNext(i, m, n);
161 while (next > i) // 若存在后继小于i说明重复,就不进行下去了（只有不重复时进入while循环）
162 next = getNext(next, m, n);
163 if (next == i) // 处理当前环
164 movedata(mtx, i, m, n);
165 }
166 }
167 /*****************矩阵原地转置END********************/
168
169 //LUP求逆(将每列b求出的各列x进行组装)
170 double * LUP_solve_inverse(double A[N*N])
171 {
172 //创建矩阵A的副本，注意不能直接用A计算，因为LUP分解算法已将其改变
173 double *A_mirror = new double[N*N]();
174 double *inv_A = new double[N*N]();//最终的逆矩阵（还需要转置）
175 double *inv_A_each = new double[N]();//矩阵逆的各列
176 //double *B =new double[N*N]();
177 double *b = new double[N]();//b阵为B阵的列矩阵分量
178
179 for (int i = 0; i < N; i++)
180 {
181 double *L = new double[N*N]();
182 double *U = new double[N*N]();
183 int *P = new int[N]();
184
185 //构造单位阵的每一列
186 for (int i = 0; i < N; i++)
187 {
188 b[i] = 0;
189 }
190 b[i] = 1;
191
192 //每次都需要重新将A复制一份
193 for (int i = 0; i < N*N; i++)
194 {
195 A_mirror[i] = A[i];
196 }
197
198 LUP_Descomposition(A_mirror, L, U, P);
199
200 inv_A_each = LUP_Solve(L, U, P, b);
201 memcpy(inv_A + i * N, inv_A_each, N * sizeof(double));//将各列拼接起来
202 }
203 transpose(inv_A, N, N);//由于现在根据每列b算出的x按行存储，因此需转置
204
205 return inv_A;
206 }