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 <stdio.h>
00034 #include <fstream>
00035 #include <iostream>
00036 #include <ctype.h>
00037 #include <ctime>
00038 #include "SceneRecog.h"
00039
00040 #include <ros/ros.h>
00041
00042 CSceneRecog *p_plane_display = NULL;
00043
00044 CSceneRecog::CSceneRecog ()
00045 {
00046 L_width_ = 0;
00047 L_height_ = 0;
00048 p_plane_display = this;
00049 plane_parameters_[0] = 0.0;
00050 plane_parameters_[1] = 0.0;
00051 plane_parameters_[2] = 0.0;
00052 plane_parameters_[3] = 0.0;
00053
00054 }
00055
00056 CSceneRecog::~CSceneRecog (void)
00057 {
00058 }
00059
00060 void CSceneRecog::srProcess (std::vector < std::vector < CPoint3d > >&data)
00061 {
00062 laser_data_.swap (data);
00063 srPlaneExtract ();
00064 srStructureExtract ();
00065 }
00066
00067 void CSceneRecog::srPlaneExtract ()
00068 {
00069 CPoint3d PointInSquare[DIMEN][DIMEN];
00070 CPoint3d Mv;
00071 CPoint3d Nvec;
00072 int flagbuf;
00073 int pnum = -1;
00074 if (laser_data_.size () > 0)
00075 {
00076 L_width_ = (int) laser_data_.size ();
00077 L_height_ = (int) laser_data_[0].size ();
00078
00079 std::vector < CPointTag > TagVector (L_height_);
00080 std::vector < std::vector < CPointTag > >PTV (L_width_, TagVector);
00081 point_tag_vector_.swap (PTV);
00082
00083 for (int i = 0; i < L_width_ - DIMEN; i = i + 1)
00084 {
00085 for (int j = 0; j < L_height_ - DIMEN; j = j + 1)
00086 {
00087 Mv.x = Mv.y = Mv.z = 0.0;
00088 for (int k = 0; k < DIMEN; k++)
00089 for (int l = 0; l < DIMEN; l++)
00090 {
00091 PointInSquare[k][l] = laser_data_[i + k][j + l];
00092 Mv.x += PointInSquare[k][l].x;
00093 Mv.y += PointInSquare[k][l].y;
00094 Mv.z += PointInSquare[k][l].z;
00095 }
00096 Mv.x = Mv.x / (DIMEN * DIMEN);
00097 Mv.y = Mv.y / (DIMEN * DIMEN);
00098 Mv.z = Mv.z / (DIMEN * DIMEN);
00099
00100 if (srIsAPlane (PointInSquare, Mv))
00101 {
00102 Nvec =srNormalVec (laser_data_[i + DIMEN - 1][j + DIMEN - 1], laser_data_[i + DIMEN - 1][j],
00103 laser_data_[i][j + DIMEN - 1], laser_data_[i][j]);
00104
00105 flagbuf = -1;
00106 for (int f = 0; f <= DIMEN - 1; f++)
00107 for (int g = 0; g <= DIMEN - 1; g++)
00108 {
00109 if (point_tag_vector_[i + f][j + g].pflag != -1)
00110 {
00111 flagbuf = point_tag_vector_[i + f][j + g].pflag;
00112 break;
00113 }
00114 }
00115
00116 if (flagbuf == -1)
00117 {
00118 pnum++;
00119 for (int m = 0; m < DIMEN; m++)
00120 {
00121 for (int n = 0; n < DIMEN; n++)
00122 {
00123 point_tag_vector_[i + m][j + n].pflag = pnum;
00124 }
00125 }
00126 Plane pTag;
00127 pTag.VecNum = 1;
00128 pTag.VecSum = Nvec;
00129 pTag.VecAve = Nvec;
00130 pTag.MeanVal = Mv;
00131 pTag.MeanSum = Mv;
00132 plane_vector_.push_back (pTag);
00133 }
00134
00135 else
00136 {
00137 if (srAngle (plane_vector_[flagbuf].VecAve, Nvec) < 0.2 )
00138 {
00139 for (int m2 = 0; m2 < DIMEN; m2++)
00140 for (int n2 = 0; n2 < DIMEN; n2++)
00141 {
00142 point_tag_vector_[i + m2][j + n2].pflag = flagbuf;
00143 }
00144
00145 plane_vector_[flagbuf].VecNum++;
00146
00147 plane_vector_[flagbuf].VecSum.x += Nvec.x;
00148 plane_vector_[flagbuf].VecSum.y += Nvec.y;
00149 plane_vector_[flagbuf].VecSum.z += Nvec.z;
00150
00151 plane_vector_[flagbuf].VecAve.x = plane_vector_[flagbuf].VecSum.x / plane_vector_[flagbuf].VecNum;
00152 plane_vector_[flagbuf].VecAve.y = plane_vector_[flagbuf].VecSum.y / plane_vector_[flagbuf].VecNum;
00153 plane_vector_[flagbuf].VecAve.z = plane_vector_[flagbuf].VecSum.z / plane_vector_[flagbuf].VecNum;
00154
00155 plane_vector_[flagbuf].MeanSum.x += Mv.x;
00156 plane_vector_[flagbuf].MeanSum.y += Mv.y;
00157 plane_vector_[flagbuf].MeanSum.z += Mv.z;
00158
00159 plane_vector_[flagbuf].MeanVal.x = plane_vector_[flagbuf].MeanSum.x / plane_vector_[flagbuf].VecNum;
00160 plane_vector_[flagbuf].MeanVal.y = plane_vector_[flagbuf].MeanSum.y / plane_vector_[flagbuf].VecNum;
00161 plane_vector_[flagbuf].MeanVal.z = plane_vector_[flagbuf].MeanSum.z / plane_vector_[flagbuf].VecNum;
00162 }
00163 }
00164 }
00165 }
00166 }
00167 }
00168 }
00169
00170 void CSceneRecog::srStructureExtract ()
00171 {
00172
00173 using namespace std;
00174
00175 for (int i = 0; i < L_height_; i++)
00176 {
00177 for (int j = 0; j < L_width_; j++)
00178 {
00179 int flag = point_tag_vector_[j][i].pflag;
00180 if (flag >= 0)
00181 {
00182 CPointIndex index (i, j);
00183 plane_vector_[flag].InnerPoints.push_back (index);
00184 }
00185 }
00186 }
00187
00188
00189
00190 int PlaneNum = (int) plane_vector_.size ();
00191 for (int i = 0; i < PlaneNum - 1; i++)
00192 {
00193 int pmax = i;
00194 for (int j = i + 1; j < PlaneNum; j++)
00195 {
00196 if (plane_vector_[j].VecNum > plane_vector_[pmax].VecNum)
00197 {
00198 pmax = j;
00199 }
00200 }
00201 if (pmax != i)
00202 std::swap (plane_vector_[i], plane_vector_[pmax]);
00203 }
00204
00205
00206
00207
00208
00209
00210
00211
00212 int iterate = 2;
00213 while (iterate-- > 1)
00214 {
00215
00216 int cnt = 0;
00217 vector < Plane > Planes;
00218 vector < Plane > ExcludePlane;
00219 for (int i = 0; i < (int) plane_vector_.size (); i++)
00220 {
00221 cnt++;
00222 if (plane_vector_[i].VecNum < 4)
00223 break;
00224
00225 plane_vector_[i].A = plane_vector_[i].VecAve.x;
00226 plane_vector_[i].B = plane_vector_[i].VecAve.y;
00227 plane_vector_[i].C = plane_vector_[i].VecAve.z;
00228 plane_vector_[i].D = -(plane_vector_[i].A * plane_vector_[i].MeanVal.x
00229 + plane_vector_[i].B * plane_vector_[i].MeanVal.y
00230 + plane_vector_[i].C * plane_vector_[i].MeanVal.z);
00231
00232 bool flag = false;
00233 float angle;
00234 for (int j = 0; j < (int) Planes.size (); j++)
00235 {
00236 angle = srAngle (plane_vector_[i].VecAve, Planes[j].VecAve);
00237 angle = angle < (PI - angle) ? angle : (PI - angle);
00238 if (angle < 20 * PI / 180)
00239 {
00240 if (srP2Pl (plane_vector_[i].MeanVal, Planes[j]) < 0.1)
00241 {
00242 Planes[j].VecNum += plane_vector_[i].VecNum;
00243
00244
00245 Planes[j].VecSum += plane_vector_[i].VecSum;
00246
00247
00248 Planes[j].VecAve.x = Planes[j].VecSum.x / Planes[j].VecNum;
00249 Planes[j].VecAve.y = Planes[j].VecSum.y / Planes[j].VecNum;
00250 Planes[j].VecAve.z = Planes[j].VecSum.z / Planes[j].VecNum;
00251
00252
00253 Planes[j].MeanSum += plane_vector_[i].MeanSum;
00254
00255
00256 Planes[j].MeanVal.x = Planes[j].MeanSum.x / Planes[j].VecNum;
00257 Planes[j].MeanVal.y = Planes[j].MeanSum.y / Planes[j].VecNum;
00258 Planes[j].MeanVal.z = Planes[j].MeanSum.z / Planes[j].VecNum;
00259
00260 Planes[j].A = Planes[j].VecAve.x;
00261 Planes[j].B = Planes[j].VecAve.y;
00262 Planes[j].C = Planes[j].VecAve.z;
00263 Planes[j].D = -(Planes[j].A * Planes[j].MeanVal.x
00264 + Planes[j].B * Planes[j].MeanVal.y
00265 + Planes[j].C * Planes[j].MeanVal.z);
00266
00267 int I = (int) plane_vector_[i].InnerPoints.size ();
00268 for (int k = 0; k < I; k++)
00269 {
00270 Planes[j].InnerPoints.push_back (plane_vector_[i].InnerPoints[k]);
00271 }
00272 flag = true;
00273 break;
00274 }
00275
00276 }
00277 }
00278
00279 if (!flag)
00280 {
00281 if ((int) Planes.size () < iterate * iterate * MAX_PLANE_NUM)
00282 {
00283 Plane ptemp;
00284 Planes.push_back (ptemp);
00285 std::swap (Planes[Planes.size () - 1], plane_vector_[i]);
00286 }
00287 else
00288 {
00289 Plane ptemp;
00290 ExcludePlane.push_back (ptemp);
00291 std::swap (ExcludePlane[ExcludePlane.size () - 1], plane_vector_[i]);
00292 }
00293 }
00294 }
00295 plane_vector_.swap (Planes);
00296 }
00297
00298 boundary_vec_.resize (plane_vector_.size ());
00299 for (int i = 0; i < (int) plane_vector_.size (); i++)
00300 {
00301 int flag = 0;
00302 boundary_vec_[i].push_back (plane_vector_[i].InnerPoints[0]);
00303 for (int j = 1; j < (int) plane_vector_[i].InnerPoints.size (); j++)
00304 {
00305 flag = 1;
00306 if (plane_vector_[i].InnerPoints[j - 1].i == plane_vector_[i].InnerPoints[j].i
00307 && plane_vector_[i].InnerPoints[j - 1].j + 1 == plane_vector_[i].InnerPoints[j].j)
00308 continue;
00309 else
00310 {
00311 boundary_vec_[i].push_back (plane_vector_[i].InnerPoints[j - 1]);
00312 boundary_vec_[i].push_back (plane_vector_[i].InnerPoints[j]);
00313 flag = 0;
00314 }
00315 }
00316 if (flag == 1)
00317 boundary_vec_[i].push_back (plane_vector_[i].InnerPoints.back ());
00318 }
00319
00320
00321
00322
00323
00324
00325
00326 convex_vec_.resize (boundary_vec_.size ());
00327 for (int i = 0; i < (int) boundary_vec_.size (); i++)
00328 {
00329 int minidx = 0;
00330 for (int j = 1; j < (int) boundary_vec_[i].size (); j++)
00331 {
00332 if (boundary_vec_[i][j].i < boundary_vec_[i][minidx].i
00333 || (boundary_vec_[i][j].i == boundary_vec_[i][minidx].i
00334 && boundary_vec_[i][j].j < boundary_vec_[i][minidx].j))
00335 minidx = j;
00336 }
00337 if (minidx != 0)
00338 std::swap (boundary_vec_[i][0], boundary_vec_[i][minidx]);
00339
00340 for (int j = 1; j < (int) boundary_vec_[i].size () - 1; j++)
00341 {
00342 int r = j;
00343 for (int k = j + 1; k < (int) boundary_vec_[i].size (); k++)
00344 {
00345 bool res = srCompL1R0bool (boundary_vec_[i][0], boundary_vec_[i][r], boundary_vec_[i][k]);
00346 if (res == 0)
00347 r = k;
00348 }
00349 if (r != j)
00350 std::swap (boundary_vec_[i][j], boundary_vec_[i][r]);
00351 }
00352
00353 std::vector < CPointIndex > stack;
00354 stack.clear ();
00355 stack.push_back (boundary_vec_[i][0]);
00356 stack.push_back (boundary_vec_[i][1]);
00357 stack.push_back (boundary_vec_[i][2]);
00358
00359 int top = 2;
00360 for (int j = 3; j < (int) boundary_vec_[i].size (); j++)
00361 {
00362 while (top >= 2 && srCross_Product (stack[top - 1], stack[top], boundary_vec_[i][j]) <= 0)
00363 {
00364 stack.pop_back ();
00365 top--;
00366 }
00367 stack.push_back (boundary_vec_[i][j]);
00368 top++;
00369 }
00370 convex_vec_[i].swap (stack);
00371 }
00372
00373
00374
00375
00376
00377
00378 plane_parameters_[0] = plane_vector_[0].A;
00379 plane_parameters_[1] = plane_vector_[0].B;
00380 plane_parameters_[2] = plane_vector_[0].C;
00381 plane_parameters_[3] = plane_vector_[0].D;
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404 }
00405
00406 float CSceneRecog::srPtopDistan (CPoint3d p1, CPoint3d p2)
00407 {
00408 return ((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
00409 }
00410
00411 bool CSceneRecog::srIsAPlane (CPoint3d p[][DIMEN], CPoint3d m)
00412 {
00413 CPoint3d Pzero (0, 0, 0);
00414 int n = 3, jt = 100;
00415 float cov[9] = { 0.0 };
00416 float s1, s2, s3, min;
00417 float eps = 0.01f;
00418 CPoint3d sum, ptemp;
00419 int zeronum = 0;
00420 bool booldis = true;
00421 float distemp, maxdis = 0.0;
00422
00423 ptemp = p[0][0];
00424 for (int i = 0; i < DIMEN; i++)
00425 {
00426 for (int j = 1; j < DIMEN; j++)
00427 {
00428 if (Pzero == p[i][j])
00429 {
00430 zeronum++;
00431 }
00432 else
00433 {
00434 distemp = srPtopDistan (ptemp, p[i][j]);
00435 if (distemp > maxdis)
00436 {
00437 maxdis = distemp;
00438 }
00439 ptemp = p[i][j];
00440 }
00441 }
00442 }
00443
00444 if (maxdis > MAXDISTH1 * MAXDISTH1)
00445 {
00446 booldis = false;
00447 return false;
00448 }
00449 for (int i = 0; i < DIMEN; i++)
00450 {
00451 for (int j = 0; j < DIMEN; j++)
00452 {
00453 float buf[3];
00454 buf[0] = p[i][j].x - m.x;
00455 buf[1] = p[i][j].y - m.y;
00456 buf[2] = p[i][j].z - m.z;
00457 for (int k = 0; k < 3; k++)
00458 for (int l = 0; l < 3; l++)
00459 if (!(p[i][j] == Pzero))
00460 {
00461 cov[k * 3 + l] += buf[k] * buf[l];
00462 }
00463 }
00464 }
00465
00466 srEigenValue (cov, n, eps, jt);
00467 s1 = fabs (cov[0]);
00468 s2 = fabs (cov[4]);
00469 s3 = fabs (cov[8]);
00470 min = s1 < s2 ? s1 : s2;
00471 min = s3 < min ? s3 : min;
00472
00473
00474 if (min < THRESH && zeronum < DIMEN && booldis)
00475 return true;
00476 return false;
00477 }
00478
00479 int CSceneRecog::srEigenValue (float a[], int n, double eps, int jt)
00480 {
00481 int i, j, u, w, t, s, l;
00482 int p = 0;
00483 int q = 0;
00484 float fm, cn, sn, omega, x, y, d, v[30];
00485 l = 1;
00486 for (i = 0; i <= n - 1; i++)
00487 {
00488 v[i * n + i] = 1.0;
00489 for (j = 0; j <= n - 1; j++)
00490 {
00491 if (i != j)
00492 {
00493 v[i * n + j] = 0.0;
00494 }
00495 }
00496 }
00497 while (l == 1)
00498 {
00499 fm = 0.0;
00500 for (i = 0; i <= n - 1; i++)
00501 {
00502 for (j = 0; j <= n - 1; j++)
00503 {
00504 d = fabs (a[i * n + j]);
00505 if ((i != j) && (d > fm))
00506 {
00507 fm = d;
00508 p = i;
00509 q = j;
00510 }
00511 }
00512 }
00513 if (fm < eps)
00514 {
00515 return (1);
00516 }
00517 if (l > jt)
00518 {
00519 return (-1);
00520 }
00521 l = l + 1;
00522 u = p * n + q;
00523 w = p * n + p;
00524 t = q * n + p;
00525 s = q * n + q;
00526 x = -a[u];
00527 y = (a[s] - a[w]) / 2.0;
00528 omega = x / sqrt (x * x + y * y);
00529 if (y < 0.0)
00530 {
00531 omega = -omega;
00532 }
00533 sn = 1.0 + sqrt (1.0 - omega * omega);
00534 sn = omega / sqrt (2.0 * sn);
00535 cn = sqrt (1.0 - sn * sn);
00536 fm = a[w];
00537 a[w] = fm * cn * cn + a[s] * sn * sn + a[u] * omega;
00538 a[s] = fm * sn * sn + a[s] * cn * cn - a[u] * omega;
00539 a[u] = 0.0;
00540 a[t] = 0.0;
00541 for (j = 0; j <= n - 1; j++)
00542 {
00543 if ((j != p) && (j != q))
00544 {
00545 u = p * n + j;
00546 w = q * n + j;
00547 fm = a[u];
00548 a[u] = fm * cn + a[w] * sn;
00549 a[w] = -fm * sn + a[w] * cn;
00550 }
00551 }
00552 for (i = 0; i <= n - 1; i++)
00553 {
00554 if ((i != p) && (i != q))
00555 {
00556 u = i * n + p;
00557 w = i * n + q;
00558 fm = a[u];
00559 a[u] = fm * cn + a[w] * sn;
00560 a[w] = -fm * sn + a[w] * cn;
00561 }
00562 }
00563 for (i = 0; i <= n - 1; i++)
00564 {
00565 u = i * n + p;
00566 w = i * n + q;
00567 fm = v[u];
00568 v[u] = fm * cn + v[w] * sn;
00569 v[w] = -fm * sn + v[w] * cn;
00570 }
00571 }
00572 return 0;
00573 }
00574
00575 CPoint3d CSceneRecog::srNormalVec (CPoint3d start1, CPoint3d end1, CPoint3d start2, CPoint3d end2)
00576 {
00577 float r1, r2;
00578 CPoint3d point1, point2, point;
00579 point1 = srVecMuti (start1, end1, start1, end2);
00580 point2 = srVecMuti (start2, end1, start2, end2);
00581 r1 = sqrt (point1.x * point1.x + point1.y * point1.y + point1.z * point1.z);
00582 r2 = sqrt (point2.x * point2.x + point2.y * point2.y + point2.z * point2.z);
00583 point.x = (point1.x / r1 + point2.x / r2) / 2;
00584 point.y = (point1.y / r1 + point2.y / r2) / 2;
00585 point.z = (point1.z / r1 + point2.z / r2) / 2;
00586 return point;
00587 }
00588
00589 CPoint3d CSceneRecog::srVecMuti (CPoint3d start1, CPoint3d end1, CPoint3d start2, CPoint3d end2)
00590 {
00591 float x1, y1, z1;
00592 float x2, y2, z2;
00593 x1 = end1.x - start1.x;
00594 y1 = end1.y - start1.y;
00595 z1 = end1.z - start1.z;
00596 x2 = end2.x - start2.x;
00597 y2 = end2.y - start2.y;
00598 z2 = end2.z - start2.z;
00599 CPoint3d point;
00600 point.x = y1 * z2 - y2 * z1;
00601 point.y = x2 * z1 - x1 * z2;
00602 point.z = x1 * y2 - x2 * y1;
00603 return point;
00604 }
00605
00606 float CSceneRecog::srAngle (CPoint3d vec1, CPoint3d vec2)
00607 {
00608 float x1, y1, z1, p1, p2, result, theta;
00609 x1 = vec1.x * vec2.x;
00610 y1 = vec1.y * vec2.y;
00611 z1 = vec1.z * vec2.z;
00612 p1 = vec1.x * vec1.x + vec1.y * vec1.y + vec1.z * vec1.z;
00613 p2 = vec2.x * vec2.x + vec2.y * vec2.y + vec2.z * vec2.z;
00614 result = (x1 + y1 + z1) / sqrt (p1 * p2);
00615 theta = acos (result);
00616 return theta;
00617 }
00618
00619 float CSceneRecog::srP2Pl (CPoint3d m, const Plane & Plane)
00620 {
00621 float dist;
00622 dist = fabs (Plane.A * m.x + Plane.B * m.y + Plane.C * m.z + Plane.D);
00623 dist = dist / sqrt (Plane.A * Plane.A + Plane.B * Plane.B + Plane.C * Plane.C);
00624 return dist;
00625 }
00626
00627 void CSceneRecog::srCalColor (int idx, std::vector < float >&rgb)
00628 {
00629 static int preidx = 1000;
00630 float step = 1;
00631 if (idx < 0)
00632 {
00633 rgb[0] = 0.5;
00634 rgb[1] = 0.5;
00635 rgb[2] = 0.5;
00636 }
00637 else if (idx != preidx)
00638 {
00639 float ratio = idx % 8;
00640 ratio = (1 - ratio / 8);
00641 switch (idx % 6)
00642 {
00643 case 0:
00644 rgb[0] = ratio * step;
00645 rgb[1] = 0;
00646 rgb[2] = 0;
00647 break;
00648 case 1:
00649 rgb[0] = 0;
00650 rgb[1] = ratio * step;
00651 rgb[2] = 0;
00652 break;
00653 case 2:
00654 rgb[0] = 0;
00655 rgb[1] = 0;
00656 rgb[2] = ratio * step;
00657 break;
00658 case 3:
00659 rgb[0] = ratio * step;
00660 rgb[1] = ratio * step;
00661 rgb[2] = 0;
00662 break;
00663 case 4:
00664 rgb[0] = ratio * step;
00665 rgb[1] = 0;
00666 rgb[2] = ratio * step;
00667 break;
00668 case 5:
00669 rgb[0] = 0;
00670 rgb[1] = ratio * step;
00671 rgb[2] = ratio * step;
00672 break;
00673 }
00674 }
00675 preidx = idx;
00676 }
00677
00678 bool CSceneRecog::srCompL1R0bool (const CPointIndex & p1, const CPointIndex & p2, const CPointIndex & p3)
00679 {
00680 bool res = 1;
00681 int temp = (p2.j - p1.j) * (p3.i - p1.i) - (p2.i - p1.i) * (p3.j - p1.j);
00682 if (temp < 0)
00683 res = 0;
00684 else if (temp == 0)
00685 {
00686 int dist1 = (p2.i - p1.i) * (p2.i - p1.i) + (p2.j - p1.j) * (p2.j - p1.j);
00687 int dist2 = (p3.i - p1.i) * (p3.i - p1.i) + (p3.j - p1.j) * (p3.j - p1.j);
00688 if (dist1 > dist2)
00689 res = 0;
00690 }
00691 return res;
00692 }
00693
00694 inline int CSceneRecog::srCross_Product (const CPointIndex & p1, const CPointIndex & p2, const CPointIndex & p3)
00695 {
00696 return (p2.j - p1.j) * (p3.i - p1.i) - (p2.i - p1.i) * (p3.j - p1.j);
00697 }