From 6462776eddd77bbf84684bef56c210eb74505762 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Mon, 22 Nov 2010 12:52:52 +0100 Subject: [PATCH] Add first version of shape-detector from Martin Synek --- src/hokuyo/shape-detect/Makefile | 14 + src/hokuyo/shape-detect/Makefile.omk | 5 + src/hokuyo/shape-detect/length_data.csv | 725 ++++++++++++++++++++++++ src/hokuyo/shape-detect/main.cc | 163 ++++++ 4 files changed, 907 insertions(+) create mode 100644 src/hokuyo/shape-detect/Makefile create mode 100644 src/hokuyo/shape-detect/Makefile.omk create mode 100644 src/hokuyo/shape-detect/length_data.csv create mode 100644 src/hokuyo/shape-detect/main.cc diff --git a/src/hokuyo/shape-detect/Makefile b/src/hokuyo/shape-detect/Makefile new file mode 100644 index 00000000..08cf5ff3 --- /dev/null +++ b/src/hokuyo/shape-detect/Makefile @@ -0,0 +1,14 @@ +# Generic directory or leaf node makefile for OCERA make framework + +ifndef MAKERULES_DIR +MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) ) +endif + +ifeq ($(MAKERULES_DIR),) +all : default +.DEFAULT:: + @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n" +else +include $(MAKERULES_DIR)/Makefile.rules +endif + diff --git a/src/hokuyo/shape-detect/Makefile.omk b/src/hokuyo/shape-detect/Makefile.omk new file mode 100644 index 00000000..33dba2ed --- /dev/null +++ b/src/hokuyo/shape-detect/Makefile.omk @@ -0,0 +1,5 @@ +# -*- makefile -*- + +bin_PROGRAMS = shape_detect + +shape_detect_SOURCES = main.cc diff --git a/src/hokuyo/shape-detect/length_data.csv b/src/hokuyo/shape-detect/length_data.csv new file mode 100644 index 00000000..0b5960f0 --- /dev/null +++ b/src/hokuyo/shape-detect/length_data.csv @@ -0,0 +1,725 @@ +0,0, +1,0, +2,0, +3,0, +4,0, +5,0, +6,0, +7,0, +8,0, +9,0, +10,0, +11,0, +12,0, +13,0, +14,0, +15,0, +16,0, +17,0, +18,0, +19,0, +20,0, +21,0, +22,0, +23,0, +24,0, +25,0, +26,0, +27,0, +28,0, +29,0, +30,0, +31,0, +32,0, +33,0, +34,0, +35,0, +36,0, +37,0, +38,0, +39,0, +40,0, +41,0, +42,0, +43,0, +44,181, +45,187, +46,189, +47,191, +48,197, +49,198, +50,207, +51,213, +52,215, +53,217, +54,222, +55,222, +56,222, +57,214, +58,212, +59,212, +60,212, +61,212, +62,207, +63,207, +64,207, +65,206, +66,206, +67,207, +68,202, +69,200, +70,190, +71,188, +72,188, +73,188, +74,191, +75,192, +76,197, +77,206, +78,209, +79,213, +80,216, +81,221, +82,226, +83,227, +84,233, +85,243, +86,245, +87,254, +88,264, +89,268, +90,268, +91,270, +92,289, +93,296, +94,307, +95,319, +96,334, +97,341, +98,356, +99,375, +100,411, +101,419, +102,435, +103,437, +104,437, +105,437, +106,437, +107,436, +108,427, +109,427, +110,421, +111,418, +112,421, +113,418, +114,418, +115,423, +116,423, +117,416, +118,417, +119,415, +120,415, +121,415, +122,417, +123,417, +124,417, +125,417, +126,417, +127,421, +128,421, +129,420, +130,420, +131,419, +132,419, +133,419, +134,419, +135,419, +136,419, +137,420, +138,420, +139,422, +140,422, +141,422, +142,420, +143,420, +144,419, +145,419, +146,420, +147,418, +148,420, +149,420, +150,420, +151,418, +152,420, +153,419, +154,422, +155,419, +156,422, +157,422, +158,425, +159,425, +160,425, +161,425, +162,426, +163,423, +164,423, +165,424, +166,424, +167,424, +168,426, +169,426, +170,427, +171,427, +172,427, +173,428, +174,427, +175,432, +176,433, +177,433, +178,433, +179,434, +180,434, +181,434, +182,434, +183,434, +184,437, +185,434, +186,437, +187,438, +188,440, +189,440, +190,441, +191,440, +192,440, +193,439, +194,439, +195,439, +196,444, +197,449, +198,449, +199,449, +200,449, +201,448, +202,448, +203,449, +204,449, +205,452, +206,455, +207,457, +208,459, +209,459, +210,460, +211,460, +212,461, +213,462, +214,461, +215,462, +216,463, +217,465, +218,465, +219,467, +220,468, +221,469, +222,473, +223,475, +224,477, +225,479, +226,481, +227,481, +228,482, +229,485, +230,486, +231,487, +232,495, +233,496, +234,496, +235,496, +236,498, +237,498, +238,506, +239,507, +240,506, +241,506, +242,506, +243,506, +244,506, +245,506, +246,526, +247,527, +248,529, +249,529, +250,533, +251,534, +252,535, +253,539, +254,544, +255,546, +256,551, +257,553, +258,557, +259,563, +260,567, +261,572, +262,575, +263,576, +264,580, +265,583, +266,585, +267,591, +268,598, +269,602, +270,605, +271,612, +272,615, +273,616, +274,620, +275,623, +276,640, +277,642, +278,651, +279,652, +280,652, +281,656, +282,656, +283,657, +284,660, +285,660, +286,660, +287,661, +288,661, +289,661, +290,663, +291,665, +292,667, +293,673, +294,695, +295,709, +296,711, +297,730, +298,730, +299,733, +300,737, +301,753, +302,776, +303,794, +304,804, +305,810, +306,818, +307,828, +308,829, +309,841, +310,843, +311,856, +312,868, +313,869, +314,888, +315,898, +316,904, +317,915, +318,922, +319,954, +320,978, +321,1010, +322,1014, +323,1035, +324,1051, +325,1070, +326,1086, +327,1116, +328,1120, +329,1138, +330,1172, +331,1193, +332,1204, +333,1225, +334,1259, +335,1290, +336,1312, +337,1328, +338,1371, +339,1391, +340,0, +341,0, +342,0, +343,7, +344,0, +345,0, +346,0, +347,0, +348,0, +349,0, +350,0, +351,0, +352,0, +353,0, +354,0, +355,0, +356,0, +357,0, +358,0, +359,0, +360,0, +361,0, +362,0, +363,0, +364,0, +365,0, +366,0, +367,0, +368,0, +369,0, +370,0, +371,0, +372,0, +373,0, +374,0, +375,0, +376,0, +377,0, +378,0, +379,0, +380,0, +381,0, +382,0, +383,0, +384,0, +385,0, +386,0, +387,0, +388,0, +389,0, +390,0, +391,0, +392,0, +393,0, +394,0, +395,0, +396,0, +397,0, +398,0, +399,0, +400,0, +401,0, +402,0, +403,0, +404,0, +405,0, +406,0, +407,0, +408,0, +409,0, +410,0, +411,0, +412,0, +413,0, +414,1741, +415,1741, +416,1720, +417,1720, +418,1710, +419,1677, +420,1627, +421,1585, +422,1585, +423,1585, +424,6, +425,6, +426,6, +427,1051, +428,1050, +429,1049, +430,1049, +431,1047, +432,1049, +433,1049, +434,4258, +435,4258, +436,4370, +437,4258, +438,4154, +439,734, +440,733, +441,732, +442,730, +443,718, +444,708, +445,708, +446,708, +447,709, +448,710, +449,710, +450,710, +451,710, +452,716, +453,716, +454,717, +455,717, +456,718, +457,721, +458,721, +459,721, +460,721, +461,721, +462,721, +463,721, +464,755, +465,755, +466,755, +467,751, +468,746, +469,743, +470,743, +471,743, +472,747, +473,774, +474,775, +475,780, +476,794, +477,796, +478,803, +479,821, +480,824, +481,828, +482,843, +483,4296, +484,4341, +485,4375, +486,4375, +487,4386, +488,4404, +489,4426, +490,4427, +491,4446, +492,4446, +493,4446, +494,1153, +495,1153, +496,1067, +497,1041, +498,1029, +499,1018, +500,992, +501,957, +502,918, +503,901, +504,899, +505,862, +506,858, +507,832, +508,803, +509,800, +510,783, +511,771, +512,761, +513,752, +514,739, +515,734, +516,722, +517,718, +518,712, +519,711, +520,710, +521,710, +522,708, +523,708, +524,714, +525,717, +526,718, +527,719, +528,719, +529,728, +530,747, +531,3540, +532,3543, +533,0, +534,0, +535,4011, +536,4011, +537,4011, +538,3980, +539,3965, +540,3933, +541,3932, +542,3918, +543,3904, +544,3886, +545,3863, +546,3856, +547,3847, +548,3801, +549,3768, +550,3744, +551,3743, +552,3732, +553,3719, +554,3686, +555,3685, +556,3673, +557,3658, +558,3650, +559,3630, +560,3622, +561,3616, +562,3607, +563,3607, +564,3594, +565,3573, +566,2358, +567,2356, +568,2342, +569,2342, +570,2332, +571,2332, +572,7, +573,0, +574,3463, +575,3463, +576,3460, +577,3455, +578,3454, +579,3444, +580,3442, +581,3432, +582,3427, +583,3425, +584,3411, +585,3410, +586,3400, +587,3392, +588,3390, +589,3373, +590,3372, +591,3372, +592,3353, +593,3347, +594,3342, +595,3337, +596,3334, +597,3332, +598,3327, +599,3314, +600,3311, +601,3307, +602,3307, +603,3303, +604,3301, +605,3300, +606,3295, +607,3291, +608,3288, +609,3282, +610,3275, +611,3275, +612,3274, +613,3272, +614,3272, +615,3271, +616,3264, +617,3264, +618,3263, +619,3263, +620,3264, +621,0, +622,0, +623,0, +624,0, +625,0, +626,0, +627,0, +628,0, +629,0, +630,0, +631,0, +632,0, +633,2096, +634,2079, +635,2078, +636,2068, +637,2068, +638,2068, +639,2073, +640,2073, +641,2080, +642,7, +643,0, +644,0, +645,0, +646,0, +647,0, +648,0, +649,0, +650,0, +651,3267, +652,3267, +653,3267, +654,3269, +655,3277, +656,3278, +657,3281, +658,3283, +659,3283, +660,3283, +661,3283, +662,3286, +663,3286, +664,3286, +665,3286, +666,3286, +667,3296, +668,0, +669,0, +670,0, +671,2825, +672,2825, +673,2825, +674,2826, +675,2832, +676,2842, +677,2850, +678,2850, +679,2842, +680,328, +681,328, +682,328, +683,328, +684,328, +685,299, +686,295, +687,277, +688,271, +689,264, +690,261, +691,257, +692,251, +693,251, +694,237, +695,233, +696,232, +697,227, +698,222, +699,219, +700,215, +701,212, +702,212, +703,209, +704,201, +705,199, +706,189, +707,186, +708,184, +709,183, +710,183, +711,177, +712,177, +713,174, +714,171, +715,170, +716,170, +717,163, +718,161, +719,160, +720,155, +721,155, +722,155, +723,154, +724,146, diff --git a/src/hokuyo/shape-detect/main.cc b/src/hokuyo/shape-detect/main.cc new file mode 100644 index 00000000..2a4d3776 --- /dev/null +++ b/src/hokuyo/shape-detect/main.cc @@ -0,0 +1,163 @@ +#include +#include +#include +#include +#include +#include +#include + +// konstanta PI +#define PI 3.14159265 + +// krok laseru (rozliseni) +#define RESOLUTION 0.331 + +// presnost (v procentech) +//#define ACCURACY 3 + +using namespace std; + +// definice datoveho typu Point +typedef struct {double x,y;} Point; + +// vypocet primek +void detect_line(vector &input_date, vector &kartez, vector &lines, double eps) { + + // pomocny bod + Point point; + + // uhel polarni souradnice fi + double fi; + + // zmerena vzdalenost + int r; + + // vypocet kartezskych souradnic + for (int i = 0; i < input_date.size()-1; i +=2) { + r = (input_date[i+1] <= 19) ? 0 : input_date[i+1]; + fi = (RESOLUTION * input_date[i] - 45) * PI/180; + + point.x = r * cos(fi); + point.y = r * sin(fi); + + point.x = (point.x == -0) ? 0 : point.x; + point.y = (point.y == -0) ? 0 : point.y; + + kartez.push_back(point); + } + + // hledani primek (usecek) pomoci smeroveho vektoru a obecne rovnice primky + // obecna rovnice: ax + by + c = 0 + + // parametr c + double c; + + // normalovy vektor a posledni bod nalezici primce + Point n_vector, end_point; + + // temp + bool tmp = true; + + // primka + double line; + + for (int i = 0; i < kartez.size()-1; i++) { + + cout << "BOD: "<< kartez[i].x << " " << kartez[i].y << endl; + + if (kartez[i].x == 0 && kartez[i].y == 0) { + tmp = true; + continue; + } + + if (tmp) { + cout << "nova primka: "; + lines.push_back(kartez[i]); + + end_point = kartez[i+1]; + + n_vector.x = -1 * (kartez[i+1].y - kartez[i].y); + n_vector.y = 1 * (kartez[i+1].x - kartez[i].x); + + c = -1*n_vector.x*kartez[i].x - n_vector.y*kartez[i].y; + + cout << "nx: " << n_vector.x << " ny: " << n_vector.y << " c: " << c << endl; + + i++; + tmp = false; + } else { + cout << "dalsi bod na primce?: nx: " << n_vector.x << " ny: " << n_vector.y << " c: " << c << endl; + + line = n_vector.x*kartez[i].x + n_vector.y*kartez[i].y + c; + + cout << "odchylka: " << line << endl; + + if (line < (0+eps) && line > (0-eps)) { + end_point = kartez[i]; + + cout << "ANO: X:" << end_point.x << " Y: " << end_point.y << endl; + + } else { + cout << "4" << endl; + lines.push_back(end_point); + + cout << "NE: X:" << end_point.x << " Y: " << end_point.y << endl; + + tmp = true; + } + } + cout << endl; + } +} + +int main(int argc, char** argv) { + + // kontrola vstupniho argumentu + if (argc < 3) { + cout << "Chyby vstupni soubor." << endl; + return 1; + } + + // vector vstupnich dat + vector input_date; + + // vector kartezskych souradnic + vector kartez; + + // vector zjistenych primek + vector lines; + + // odchylka bodu od primky + double eps = atof(argv[2]); + + // nacteni vstupniho souboru - mapy + ifstream infile (argv[1], ios_base::in); + + string line; // radek vstupniho souboru + int number; // cislo prectene ze souboru + int size_line; // pocet znaku na radku + + while (getline(infile, line, ',')) { + if (line != "\n") { + stringstream strStream(line); + strStream >> number; + input_date.push_back(number); + } + } + + detect_line(input_date, kartez, lines, eps); + + // ulozeni do souboru + FILE *file = fopen("output_data","w"); +/* + for (int i = 0; i < kartez.size(); i += 1) + fprintf(file, "%f, %f\n", kartez[i].x, kartez[i].y); +*/ + for (int i = 0; i < lines.size(); i += 1) + fprintf(file, "%f, %f\n", lines[i].x, lines[i].y); + + fclose(file); + + return 0; +} + -- 2.39.2