| 提交 | 用户 | age | ||
| 842bb6 | 1 | #include "LindarPos.h" |
| Q | 2 | #include "OrdLidar.h" |
| 3 | #include "KMachine.h" | |
| 4 | ||
| 5 | int ProcessPos(int nLidarIdx, stLidarDot * pLindarDots, int nCount) | |
| 6 | { | |
| 7 | int nValidCount0 = nCount; | |
| 8 | int minDisIndex = 0; int minDistance = 55555; | |
| 9 | int minZ = 10000; int minZIndex= -1; | |
| 10 | int planeCount =0; // 最底层平面的点数量。 | |
| 11 | int firstmin = 1000,firstmax = -1000; | |
| 12 | int secondmin = 1000, secondmax = -1000; | |
| 13 | ||
| 14 | int firstCount=0; int secondCount =0; int midCount =0; | |
| 15 | results[0]=0; | |
| 16 | // 求 minZ 和 minDistance; | |
| 17 | for (int j = 0;j < nValidCount0;j++){ | |
| 18 | if (pLindarDots[j].distance < 50) continue; // skip too small points; | |
| 19 | if (pLindarDots[j].y < 40) continue; // skip too small points; | |
| 20 | if (pLindarDots[j].x < -200 || pLindarDots[j].x > 500) continue; | |
| 21 | if (pLindarDots[j].y < minZ) { minZIndex = j; minZ = pLindarDots[j].y; } | |
| 22 | if (pLindarDots[j].distance < minDistance) { | |
| 23 | minDisIndex = j; minDistance = pLindarDots[j].distance; | |
| 24 | } | |
| 25 | } | |
| 26 | ||
| 27 | for (int j = nValidCount0 -1 ;j>=0 && minZ < 500;j--) | |
| 28 | { | |
| 29 | int x = pLindarDots[j].x; | |
| 30 | int y = pLindarDots[j].y; | |
| 31 | int d = pLindarDots[j].distance; | |
| 32 | ||
| 33 | if (d < 50) continue; // skip too small points; | |
| 34 | if (y < 40) continue; // skip too small points; | |
| 35 | if (x < -200 || x > 500) continue; | |
| 36 | // 只查找 底部 50mm 数据 | |
| 37 | ||
| 38 | //results[0]++; | |
| 39 | if (y >= minZ + 50) { | |
| 40 | if ((firstCount >0 && secondCount == 0) | |
| 41 | || (secondCount >0 && firstCount == 0)) { | |
| 42 | midCount++; | |
| 43 | } | |
| 44 | continue; | |
| 45 | } | |
| 46 | ||
| 47 | if (planeCount < 40) { | |
| 48 | KMem.WDT[40+2 * planeCount]=d; | |
| 49 | KMem.WDT[40+2 * planeCount +1 ]=x; | |
| 50 | } | |
| 51 | ||
| 52 | planeCount++; | |
| 53 | if (x > -200 && x < 500) | |
| 54 | { | |
| 55 | if (x < firstmin) firstmin = x; | |
| 56 | if (x > secondmax) secondmax = x; | |
| 57 | } | |
| 58 | // 后沿 | |
| 59 | if (x > -200 && x < 100 && y < minZ + 40 ) | |
| 60 | { | |
| 61 | if (x > firstmax) firstmax = x; | |
| 62 | firstCount++; | |
| 63 | } | |
| 64 | // 前沿 | |
| 65 | if (x > 200 && x < 500 && y < minZ + 40 ) | |
| 66 | { | |
| 67 | if (x < secondmin) secondmin = x; | |
| 68 | secondCount ++; | |
| 69 | } | |
| 70 | ||
| 71 | } | |
| 72 | results[0]=planeCount; | |
| 73 | results[1]=firstCount; | |
| 74 | results[2]=midCount; | |
| 75 | results[3]=secondCount; | |
| 76 | ||
| 77 | results[4]=firstmin; | |
| 78 | results[5]=firstmax; | |
| 79 | results[6]=secondmin; | |
| 80 | results[7]=secondmax; | |
| 81 | int avg=0; | |
| 82 | if (firstCount > 0 && secondCount > 0 && midCount > 0 ){ | |
| 83 | avg = (secondmin + firstmax) / 2; | |
| 84 | }else if (planeCount>10) { | |
| 85 | avg = (firstmin + secondmax) /2; | |
| 86 | } | |
| 87 | ||
| 88 | nPosX = (nPosX *3 + avg) /4; | |
| 89 | if (minZ < 9999) nPosZ1 = (minZ + nPosZ1 * 3)/4; | |
| 90 | return 0; | |
| 91 | ||
| 92 | ||
| 93 | ||
| 94 | } | |
