标签：11 mtx int double 矩阵 PCL new include
-
计算旋转矩阵的逆矩阵，应用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 }
标签：11,mtx,int,double,矩阵,PCL,new,include 来源： https://www.cnblogs.com/beautifulmoonlight/p/14951754.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。