00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "camera_laser_calibration/white_black_grid.h"
00034
00035 CWhiteBlackGrid::CWhiteBlackGrid (void)
00036 {
00037 scene_cnt_ = 0;
00038 m_scan_mode_ = 1;
00039 }
00040
00041 CWhiteBlackGrid::~CWhiteBlackGrid (void)
00042 {
00043 }
00044
00045 void CWhiteBlackGrid::whgRegisterScanMode (bool scan_mode)
00046 {
00047 m_scan_mode_ = scan_mode;
00048 }
00049
00050 void CWhiteBlackGrid::whgNodeDetect (int _m, int _n, float *_parameters,
00051 std::vector < std::vector < CPoint3d > >&laser_data_vec)
00052 {
00053 size_t k_num;
00054 k_num = (size_t) _m *_n;
00055
00056
00057 size_t m, n;
00058 m = _m;
00059 n = _n;
00060 float A = _parameters[0];
00061 float B = _parameters[1];
00062 float C = _parameters[2];
00063 float D = _parameters[3];
00064 bin_vec_.resize (m);
00065 for (size_t p = 0; p < m; p++)
00066 {
00067 bin_vec_[p].resize (n);
00068 for (size_t q = 0; q < n; q++)
00069 {
00070 double d =
00071 abs (laser_data_vec[p][q].x * A + laser_data_vec[p][q].y * B + laser_data_vec[p][q].z * C + D) / sqrt (A * A +
00072 B * B +
00073 C * C);
00074 if (d > 0.15)
00075 {
00076 bin_vec_[p][q] = 0;
00077 }
00078 else
00079 bin_vec_[p][q] = 255;
00080 }
00081 }
00082
00083 int iChoosed = 0;
00084 bool isFound = false;
00085
00086 for (size_t i = 0; i < n; i++)
00087 {
00088 for (size_t j = 0; j < m; j++)
00089 {
00090 if (bin_vec_[j][i] == 255)
00091 {
00092 isFound = true;
00093 continue;
00094 }
00095 else
00096 {
00097 isFound = false;
00098 break;
00099 }
00100 }
00101 if (isFound)
00102 {
00103 iChoosed = i;
00104 break;
00105 }
00106 }
00107
00108 std::vector < int >Bin_ (n - iChoosed);
00109 std::vector < std::vector < int > >BinNew (m);
00110 std::vector < CPoint3d > LaserOne (n - iChoosed);
00111 std::vector < std::vector < CPoint3d > >LaserData_ (m);
00112
00113 for (size_t j = 0; j < m; j++)
00114 {
00115 for (size_t i = iChoosed, i_ = 0; i < n; i++, i_++)
00116 {
00117 Bin_[i_] = bin_vec_[j][i];
00118 LaserOne[i_] = laser_data_vec[j][i];
00119 }
00120 BinNew[j] = Bin_;
00121 LaserData_[j] = LaserOne;
00122 }
00123 bin_vec_.swap (BinNew);
00124 laser_data_vec.swap (LaserData_);
00125 n = n - iChoosed;
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 std::vector < BinIndex > centerij;
00146 BinIndex tmpindex;
00147 size_t rem;
00148
00149 size_t DELTR;
00150 size_t DELTL;
00151
00152 if (m_scan_mode_)
00153 {
00154 rem = m % 4;
00155 if (rem != 0)
00156 {
00157 m = m - rem;
00158 }
00159
00160 rem = n % 5;
00161 if (rem != 0)
00162 {
00163 n = n - rem;
00164 }
00165 DELTR = m / 4;
00166 DELTL = n / 5;
00167 }
00168 else
00169 {
00170 rem = m % 5;
00171 if (rem != 0)
00172 {
00173 m = m - rem;
00174 }
00175
00176 rem = n % 4;
00177 if (rem != 0)
00178 {
00179 n = n - rem;
00180 }
00181 DELTR = m / 5;
00182 DELTL = n / 4;
00183 }
00184
00185 std::vector < size_t > rownum;
00186 rownum.resize (m);
00187 std::vector < size_t > linenum;
00188 linenum.resize (n);
00189 std::vector < size_t > maxindex;
00190
00191 for (size_t p = 0; p < m; p += DELTR)
00192 {
00193 for (size_t q = 0; q < n; q += DELTL)
00194 {
00195 for (size_t j = 0; j < m; j++)
00196 {
00197 rownum[j] = 0;
00198 }
00199 for (size_t i = 0; i < n; i++)
00200 {
00201 linenum[i] = 0;
00202 }
00203
00204
00205 for (size_t j = p; j < p + DELTR; j++)
00206 {
00207 for (size_t i = q; i < q + DELTL; i++)
00208 {
00209 if ((bin_vec_[j][i] == 0))
00210 {
00211 rownum[j] += 1;
00212 }
00213 }
00214 }
00215
00216 size_t maxj = p, maxvaluej = 0;
00217 for (size_t j = p; j < p + DELTR; j++)
00218 {
00219 if (rownum[j] >= maxvaluej)
00220 {
00221 maxvaluej = rownum[j];
00222 maxj = j;
00223 }
00224 }
00225
00226 std::vector < size_t > vec_ (0);
00227 maxindex.swap (vec_);
00228 for (size_t j = p; j < p + DELTR; j++)
00229 {
00230 if (rownum[j] == maxvaluej)
00231 maxindex.push_back (j);
00232 }
00233 size_t size_maxj = (size_t) maxindex.size ();
00234 size_t summaxj = 0;
00235 for (size_t k = 0; k < size_maxj; k++)
00236 {
00237 summaxj += maxindex[k];
00238 }
00239 float quo;
00240 size_t avemaxj;
00241 quo = summaxj / size_maxj;
00242 if ((quo - (size_t) quo) < 0.5)
00243 avemaxj = (size_t) quo;
00244 else
00245 avemaxj = (size_t) quo + 1;
00246
00247 for (size_t i = q; i < q + DELTL; i++)
00248 {
00249 for (size_t j = p; j < p + DELTR; j++)
00250 {
00251 if ((bin_vec_[j][i] == 0))
00252 {
00253 linenum[i] += 1;
00254 }
00255 }
00256 }
00257
00258
00259 size_t maxi = q, maxvaluei = 0;
00260 for (size_t i = q; i < q + DELTL; i++)
00261 {
00262 if (linenum[i] >= maxvaluei)
00263 {
00264 maxvaluei = linenum[i];
00265 maxi = i;
00266 }
00267 }
00268
00269
00270 maxindex.clear ();
00271 for (size_t i = q; i < q + DELTL; i++)
00272 {
00273 if (linenum[i] == maxvaluei)
00274 maxindex.push_back (i);
00275 }
00276 size_t size_maxi = (size_t) maxindex.size ();
00277 size_t summaxi = 0;
00278 for (size_t k = 0; k < size_maxi; k++)
00279 {
00280 summaxi += maxindex[k];
00281 }
00282 size_t avemaxi;
00283 quo = summaxi / size_maxi;
00284 if ((quo - (size_t) quo) < 0.5)
00285 avemaxi = (size_t) quo;
00286 else
00287 avemaxi = (size_t) quo + 1;
00288
00289
00290
00291 tmpindex.bin_j = avemaxj;
00292 tmpindex.bin_i = avemaxi;
00293 centerij.push_back (tmpindex);
00294 }
00295 }
00296
00297 std::vector < CPoint3d > Node_ (0);
00298 node_vec_.swap (Node_);
00299 if (m_scan_mode_)
00300 {
00301 for (int k = 0; k < 3; k++)
00302 {
00303 for (int i = 5 * k; i < 5 * k + 4; i++)
00304 {
00305 BinIndex avenode = whgAverageof4index (centerij[i], centerij[i + 1], centerij[i + 5], centerij[i + 6]);
00306 CPoint3d ptmp;
00307 ptmp.x = laser_data_vec[avenode.bin_j][avenode.bin_i].x;
00308 ptmp.y = laser_data_vec[avenode.bin_j][avenode.bin_i].y;
00309 ptmp.z = laser_data_vec[avenode.bin_j][avenode.bin_i].z;
00310 node_vec_.push_back (ptmp);
00311 }
00312 }
00313 }
00314 else
00315 {
00316 for (int k = 0; k < 4; k++)
00317 {
00318 for (int i = 4 * k; i < 4 * k + 3; i++)
00319 {
00320 BinIndex avenode = whgAverageof4index (centerij[i], centerij[i + 1], centerij[i + 4], centerij[i + 5]);
00321 CPoint3d ptmp;
00322 ptmp.x = laser_data_vec[avenode.bin_j][avenode.bin_i].x;
00323 ptmp.y = laser_data_vec[avenode.bin_j][avenode.bin_i].y;
00324 ptmp.z = laser_data_vec[avenode.bin_j][avenode.bin_i].z;
00325 node_vec_.push_back (ptmp);
00326 }
00327 }
00328
00329 }
00330 }
00331
00332
00333 BinIndex CWhiteBlackGrid::whgAverageof4index (BinIndex bi1, BinIndex bi2, BinIndex bi3, BinIndex bi4)
00334 {
00335 int av_j, av_i, sum_i = 0, sum_j = 0;
00336 sum_j = bi1.bin_j + bi2.bin_j + bi3.bin_j + bi4.bin_j;
00337 sum_i = bi1.bin_i + bi2.bin_i + bi3.bin_i + bi4.bin_i;
00338 float quoj, quoi;
00339 quoj = sum_j / 4;
00340 if ((quoj - (int) quoj) < 0.5)
00341 {
00342 av_j = (int) quoj;
00343 }
00344 else
00345 {
00346 av_j = (int) quoj + 1;
00347 }
00348
00349 quoi = sum_i / 4;
00350 if ((quoi - (int) quoi) < 0.5)
00351 {
00352 av_i = (int) quoi;
00353 }
00354 else
00355 {
00356 av_i = (int) quoi + 1;
00357 }
00358 BinIndex tmp;
00359 tmp.bin_j = av_j;
00360 tmp.bin_i = av_i;
00361 return (tmp);
00362 }
00363
00364 void CWhiteBlackGrid::whgNodeModify (void)
00365 {
00366 int nump = (int) node_vec_.size ();
00367 match_pair_vec_.resize (nump);
00368 for (int i = 0; i < nump; i++)
00369 {
00370 match_pair_vec_[i].x = node_vec_[i].x;
00371 match_pair_vec_[i].y = node_vec_[i].y;
00372 match_pair_vec_[i].z = node_vec_[i].z;
00373 }
00374
00375 standard_vec_.resize (12);
00376 if (m_scan_mode_)
00377 {
00378 standard_vec_[0] = CPoint3d (0.75, 0.00, 0);
00379 standard_vec_[1] = CPoint3d (0.50, 0.00, 0);
00380 standard_vec_[2] = CPoint3d (0.25, 0.00, 0);
00381 standard_vec_[3] = CPoint3d (0, 0.00, 0);
00382
00383 standard_vec_[4] = CPoint3d (0.75, 0.25, 0);
00384 standard_vec_[5] = CPoint3d (0.50, 0.25, 0);
00385 standard_vec_[6] = CPoint3d (0.25, 0.25, 0);
00386 standard_vec_[7] = CPoint3d (0, 0.25, 0);
00387
00388 standard_vec_[8] = CPoint3d (0.75, 0.50, 0);
00389 standard_vec_[9] = CPoint3d (0.50, 0.50, 0);
00390 standard_vec_[10] = CPoint3d (0.25, 0.50, 0);
00391 standard_vec_[11] = CPoint3d (0, 0.50, 0);
00392 }
00393 else
00394 {
00395 standard_vec_[0] = CPoint3d (0, 0, 0);
00396 standard_vec_[1] = CPoint3d (0, 0.25, 0);
00397 standard_vec_[2] = CPoint3d (0, 0.50, 0);
00398
00399 standard_vec_[3] = CPoint3d (0.25, 0, 0);
00400 standard_vec_[4] = CPoint3d (0.25, 0.25, 0);
00401 standard_vec_[5] = CPoint3d (0.25, 0.50, 0);
00402
00403 standard_vec_[6] = CPoint3d (0.50, 0, 0);
00404 standard_vec_[7] = CPoint3d (0.50, 0.25, 0);
00405 standard_vec_[8] = CPoint3d (0.50, 0.50, 0);
00406
00407 standard_vec_[9] = CPoint3d (0.75, 0, 0);
00408 standard_vec_[10] = CPoint3d (0.75, 0.25, 0);
00409 standard_vec_[11] = CPoint3d (0.75, 0.50, 0);
00410 }
00411 whgCalculateTransformationRT ();
00412 }
00413
00414 void CWhiteBlackGrid::whgCalculateTransformationRT (void)
00415 {
00416 int n = (int) match_pair_vec_.size ();
00417 VectorXd avr_m (3), avr_d (3);
00418 MatrixXd matrix_M (n, 3), matrix_D (n, 3);
00419 MatrixXd matrix_MM (n, 3), matrix_DD (n, 3);
00420 MatrixXd matrix_S (3, 3);
00421 MatrixXd matrix_N (4, 4);
00422
00423 MatrixXd Q;
00424 VectorXd vector_q (4);
00425 VectorXd vector_c;
00426
00427 MatrixXd matrix_MyR (3, 3);
00428 VectorXd vector_Myt (3);
00429
00430 for (int i = 0; i < n; i++)
00431 {
00432 matrix_M (i, 0) = match_pair_vec_[i].x;
00433 matrix_M (i, 1) = match_pair_vec_[i].y;
00434 matrix_M (i, 2) = match_pair_vec_[i].z;
00435
00436 matrix_D (i, 0) = standard_vec_[i].x;
00437 matrix_D (i, 1) = standard_vec_[i].y;
00438 matrix_D (i, 2) = standard_vec_[i].z;
00439 }
00440
00441 avr_m[0] = 0.0;
00442 avr_m[1] = 0.0;
00443 avr_m[2] = 0.0;
00444
00445 avr_d[0] = 0.0;
00446 avr_d[1] = 0.0;
00447 avr_d[2] = 0.0;
00448
00449 for (int i = 0; i < n; i++)
00450 {
00451 avr_m[0] += match_pair_vec_[i].x;
00452 avr_m[1] += match_pair_vec_[i].y;
00453 avr_m[2] += match_pair_vec_[i].z;
00454
00455 avr_d[0] += standard_vec_[i].x;
00456 avr_d[1] += standard_vec_[i].y;
00457 avr_d[2] += standard_vec_[i].z;
00458 }
00459 avr_m[0] /= n;
00460 avr_m[1] /= n;
00461 avr_m[2] /= n;
00462
00463 avr_d[0] /= n;
00464 avr_d[1] /= n;
00465 avr_d[2] /= n;
00466
00467 for (int i = 0; i < n; i++)
00468 {
00469 matrix_MM (i, 0) = matrix_M (i, 0) - avr_m[0];
00470 matrix_MM (i, 1) = matrix_M (i, 1) - avr_m[1];
00471 matrix_MM (i, 2) = matrix_M (i, 2) - avr_m[2];
00472
00473 matrix_DD (i, 0) = matrix_D (i, 0) - avr_d[0];
00474 matrix_DD (i, 1) = matrix_D (i, 1) - avr_d[1];
00475 matrix_DD (i, 2) = matrix_D (i, 2) - avr_d[2];
00476 }
00477
00478 for (int i = 0; i < 3; i++)
00479 {
00480 for (int j = 0; j < 3; j++)
00481 {
00482 matrix_S (i, j) = 0.0;
00483 }
00484 }
00485
00486 for (int i = 0; i < 3; i++)
00487 {
00488 for (int j = 0; j < 3; j++)
00489 {
00490 for (int k = 0; k < n; k++)
00491 {
00492 matrix_S (i, j) += matrix_MM (k, i) * matrix_DD (k, j);
00493 }
00494 }
00495 }
00496
00497 matrix_N (0, 0) = matrix_S (0, 0) + matrix_S (1, 1) + matrix_S (2, 2);
00498
00499 matrix_N (0, 1) = matrix_N (1, 0) = matrix_S (1, 2) - matrix_S (2, 1);
00500 matrix_N (0, 2) = matrix_N (2, 0) = matrix_S (2, 0) - matrix_S (0, 2);
00501 matrix_N (0, 3) = matrix_N (3, 0) = matrix_S (0, 1) - matrix_S (1, 0);
00502
00503 matrix_N (1, 1) = matrix_S (0, 0) - matrix_S (1, 1) - matrix_S (2, 2);
00504
00505 matrix_N (1, 2) = matrix_N (2, 1) = matrix_S (0, 1) + matrix_S (1, 0);
00506 matrix_N (1, 3) = matrix_N (3, 1) = matrix_S (2, 0) + matrix_S (0, 2);
00507 matrix_N (3, 2) = matrix_N (2, 3) = matrix_S (1, 2) + matrix_S (2, 1);
00508
00509 matrix_N (2, 2) = -matrix_S (0, 0) + matrix_S (1, 1) - matrix_S (2, 2);
00510 matrix_N (3, 3) = -matrix_S (0, 0) - matrix_S (1, 1) + matrix_S (2, 2);
00511
00512
00513
00514 EigenSolver < MatrixXd > es (matrix_N);
00515
00516 double lambda[4] = { 0 };
00517 for (int i = 0; i < 4; ++i)
00518 {
00519 lambda[i] = real (es.eigenvalues ()[i]);
00520 }
00521 double max1_ = max (lambda[0], lambda[1]);
00522 double max2_ = max (lambda[2], lambda[3]);
00523 double max_ = max (max1_, max2_);
00524 int j = 0;
00525 for (; j < 4; j++)
00526 {
00527 if (lambda[j] == max_)
00528 {
00529 break;
00530 }
00531 }
00532 for (int i = 0; i < 4; i++)
00533 {
00534 vector_q[i] = real (es.eigenvectors ().col (j)[i]);
00535 }
00536
00537
00538 matrix_MyR (0, 0) = pow (vector_q[0], 2) + pow (vector_q[1], 2) - pow (vector_q[2], 2) - pow (vector_q[3], 2);
00539 matrix_MyR (0, 1) = 2 * (vector_q[1] * vector_q[2] - vector_q[3] * vector_q[0]);
00540 matrix_MyR (0, 2) = 2 * (vector_q[1] * vector_q[3] + vector_q[2] * vector_q[0]);
00541
00542 matrix_MyR (1, 0) = 2 * (vector_q[1] * vector_q[2] + vector_q[3] * vector_q[0]);
00543 matrix_MyR (1, 1) = pow (vector_q[0], 2) - pow (vector_q[1], 2) + pow (vector_q[2], 2) - pow (vector_q[3], 2);
00544 matrix_MyR (1, 2) = 2 * (vector_q[2] * vector_q[3] - vector_q[1] * vector_q[0]);
00545
00546 matrix_MyR (2, 0) = 2 * (vector_q[3] * vector_q[1] - vector_q[2] * vector_q[0]);
00547 matrix_MyR (2, 1) = 2 * (vector_q[3] * vector_q[2] + vector_q[1] * vector_q[0]);
00548 matrix_MyR (2, 2) = pow (vector_q[0], 2) - pow (vector_q[1], 2) - pow (vector_q[2], 2) + pow (vector_q[3], 2);
00549
00550 MatrixXd matrix_MyRR;
00551 matrix_MyRR = matrix_MyR.inverse ();
00552
00553
00554 vector_c = matrix_MyRR * avr_d;
00555 vector_Myt = avr_m - vector_c;
00556
00557 whgTransform (matrix_MyRR, vector_Myt, standard_vec_);
00558
00559
00560 ofstream outf;
00561 char filename[100] = "laserpts.txt";
00562
00563 outf.open (filename);
00564 if (outf.fail ())
00565 {
00566 cout << "can not open file";
00567
00568 return;
00569 }
00570
00571 for (size_t i = 0; i < standard_vec_.size (); i++)
00572 {
00573 outf.setf (ios::right);
00574 outf.width (8);
00575 outf << standard_vec_[i].x << " " << standard_vec_[i].y << " " << standard_vec_[i].z << endl;
00576 }
00577 scene_cnt_++;
00578
00579 outf.close ();
00580 }
00581
00582 void CWhiteBlackGrid::whgTransform (MatrixXd matrix_R, VectorXd vector_t, std::vector < CPoint3d > &laser_point_vec)
00583 {
00584 VectorXd vector_c;
00585 VectorXd vector_d (3);
00586 int n = (int) laser_point_vec.size ();
00587 for (int i = 0; i < n; i++)
00588 {
00589 vector_d[0] = laser_point_vec[i].x;
00590 vector_d[1] = laser_point_vec[i].y;
00591 vector_d[2] = laser_point_vec[i].z;
00592
00593
00594 vector_c = matrix_R * vector_d;
00595 vector_d = vector_c + vector_t;
00596
00597 laser_point_vec[i].x = vector_d[0];
00598 laser_point_vec[i].y = vector_d[1];
00599 laser_point_vec[i].z = vector_d[2];
00600 }
00601 }