• 计算旋转矩阵的逆矩阵，应用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 }