11：C++搭配PCL計算點雲旋轉矩陣逆矩陣
阿新 • • 發佈：2021-06-29
-
計算旋轉矩陣的逆矩陣，應用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> 16using 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], doubleL[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 }