Go to the documentation of this file.00001 #include <math.h>
00002 #include <vector>
00003 #include <stdio.h>
00004 #include "place_matcher_hist/hist.h"
00005 #include "geometry_msgs/PolygonStamped.h"
00006 #include "geometry_msgs/Point32.h"
00007 #define SHIFT 5
00008
00009 Hist hist(const sensor_msgs::LaserScan& scan, int anglebins)
00010 {
00011 int angleHistBin = anglebins;
00012 double bin_width = angleHistBin / ( 2*M_PI);
00013 Hist hist(angleHistBin,0);
00014 double px1;
00015 double px2;
00016 double py1;
00017 double py2;
00018 for (unsigned int shift = 1 ; shift < SHIFT; shift++)
00019 {
00020 px1 = scan.ranges[0] * sin(0 * scan.angle_increment + scan.angle_min);
00021 py1 = -scan.ranges[0] * cos(0 * scan.angle_increment + scan.angle_min);
00022
00023 for(unsigned int i = 1; i < scan.ranges.size() - SHIFT ; i++)
00024 {
00025 px2 = scan.ranges[i+SHIFT] * sin((i+SHIFT) * scan.angle_increment + scan.angle_min);
00026 py2 = -scan.ranges[i+SHIFT] * cos((i+SHIFT) * scan.angle_increment + scan.angle_min);
00027 const double distance = std::sqrt((py1 - py2) * (py1 - py2) + (px1 - px2) * (px1 - px2));
00028 if (distance < 0.1)
00029 {
00030 double ath = std::atan2(py1 - py2, px1 - px2);
00031 if (ath <0)
00032 {
00033 ath= ath + 2*M_PI;
00034 }
00035 hist[(int) floor(ath * bin_width)] += 1;
00036 }
00037 px1 = px2;
00038 py1 = py2;
00039 px1 = scan.ranges[i] * sin((i) * scan.angle_increment + scan.angle_min);
00040 py1 = -scan.ranges[i] * cos((i) * scan.angle_increment + scan.angle_min);
00041 }
00042 }
00043 return hist;
00044 }
00045
00046 int crossCorrelationHist(Hist x , Hist y)
00047 {
00048 double max = 0.0;
00049 int maxind = 0;
00050 for (unsigned int shift = 0; shift < x.size(); shift++)
00051 {
00052 double sum = 0;
00053 for (unsigned int i = 0; i < x.size(); i++)
00054 {
00055 sum += x[i] * y[(i + shift) % y.size()];
00056 }
00057 if (sum > max)
00058 {
00059 max = sum;
00060 maxind = shift;
00061 }
00062 }
00063 return maxind;
00064 }
00065
00066
00067 geometry_msgs::Pose2D localize(sensor_msgs::LaserScan a, sensor_msgs::LaserScan b, int rot_bin_count)
00068 {
00069 double trans_bin_width = TRANSCOEF;
00070 double resolution = 2 * M_PI / a.ranges.size();
00071 double rot_bin_width = rot_bin_count / (2 * M_PI);
00072
00073 Hist histA = hist(a, rot_bin_count);
00074 Hist histB = hist(b, rot_bin_count);
00075 int angleIndex = crossCorrelationHist(histA,histB);
00076 int histAngleMaxIndex=0;
00077 int histMax = 0;
00078 for (unsigned int i = 0; i < histA.size(); i++)
00079 {
00080 if (histMax < histA[i]) {
00081 histMax = histA[i];
00082 histAngleMaxIndex = i;
00083 }
00084 }
00085 int histAngleMaxIndexB=0;
00086 int histMaxB = 0;
00087 for (unsigned int i = 0; i < histB.size(); i++)
00088 {
00089 if (histMaxB < histB[i]) {
00090 histMaxB = histB[i];
00091 histAngleMaxIndexB = i;
00092 }
00093 }
00094 std::cout << "hist angle A " << histAngleMaxIndex << " B " << histAngleMaxIndexB << " cross " << angleIndex << " diff " << histAngleMaxIndex - histAngleMaxIndexB << std::endl;
00095
00096
00097 Hist histax (HISTSIZE ,0);
00098 Hist histay (HISTSIZE ,0);
00099 for (unsigned int i = 0; i < a.ranges.size(); i++)
00100 {
00101 if (a.ranges[i] < 7.9)
00102 {
00103 histax[(int) floor(trans_bin_width * a.ranges[i] * cos(a.angle_min + i * resolution - histAngleMaxIndex / rot_bin_width)) + HISTSIZEHALF]++;
00104 histay[(int) floor(trans_bin_width * a.ranges[i] * sin(a.angle_min + i * resolution - histAngleMaxIndex / rot_bin_width)) + HISTSIZEHALF]++;
00105 }
00106 }
00107
00108 Hist histbx (HISTSIZE ,0);
00109 Hist histby (HISTSIZE ,0);
00110 for (unsigned int i = 0; i < b.ranges.size(); i++)
00111 {
00112 if (b.ranges[i] < 7.9)
00113 {
00114 histbx[(int) floor(trans_bin_width * b.ranges[i] * cos(b.angle_min + i * resolution - histAngleMaxIndex / rot_bin_width - (angleIndex) / rot_bin_width)) + HISTSIZEHALF]++;
00115 histby[(int) floor(trans_bin_width * b.ranges[i] * sin(b.angle_min + i * resolution - histAngleMaxIndex / rot_bin_width - (angleIndex) / rot_bin_width)) + HISTSIZEHALF]++;
00116 }
00117 }
00118 int xIndex = crossCorrelationHist(histax, histbx);
00119 int yIndex = crossCorrelationHist(histay, histby);
00120 if (xIndex > HISTSIZEHALF) xIndex -= HISTSIZE;
00121 if (yIndex > HISTSIZEHALF) yIndex -= HISTSIZE;
00122 std::cout << xIndex/trans_bin_width << " " << yIndex/trans_bin_width << " angle index " << angleIndex/rot_bin_width << " max hist angle " << histAngleMaxIndex/rot_bin_width << "cos " << cos(-histAngleMaxIndex / rot_bin_width) << " sin " << sin(-histAngleMaxIndex/rot_bin_width) << "\n";
00123 geometry_msgs::Pose2D ret ;
00124
00125
00126 double x = xIndex / trans_bin_width;
00127 double y = yIndex / trans_bin_width;
00128 double cosin = cos(histAngleMaxIndex/rot_bin_width);
00129 double sinus = sin(histAngleMaxIndex/rot_bin_width);
00130 double xx = cosin*x - sinus* y;
00131 double yy = sinus*x + cosin*y;
00132 std::cout << "x " << x << " y " << y << " cos " << cosin << " sin " << sinus << " xx " <<xx << " YY " << yy<< std::endl;
00133 double tx = (cos(histAngleMaxIndex /rot_bin_width) * (xIndex / trans_bin_width)) - (sin( histAngleMaxIndex) * (yIndex / trans_bin_width));
00134 double ty = (sin(histAngleMaxIndex /rot_bin_width) * (xIndex / trans_bin_width)) + (cos( histAngleMaxIndex) * (yIndex / trans_bin_width));
00135 std::cout << " tx ty " << tx << " " << ty << std::endl;
00136 ret.x = -yy;
00137 ret.y = -xx;
00138 ret.theta= 2*M_PI-angleIndex/rot_bin_width;
00139
00140
00141
00142 return ret;
00143 }
00144
00145 double error(sensor_msgs::LaserScan scan, sensor_msgs::LaserScan reference, geometry_msgs::Pose2D goal)
00146 {
00147 double rx, ry,sx,sy;
00148 double error = 0;
00149 double distance ;
00150 double min = 9000000;
00151 int count = 0;
00152 geometry_msgs::PolygonStamped poly;
00153 poly.header.frame_id="/mf";
00154 geometry_msgs::PolygonStamped poly2;
00155 poly2.header.frame_id="/mf";
00156 geometry_msgs::Point32 p;
00157 for (unsigned int i = 0 ; i < reference.ranges.size(); i++) {
00158 min = 90000000;
00159 if (reference.ranges[i] < 7.9) {
00160 rx = reference.ranges[i] * sin( (i * reference.angle_increment) + reference.angle_min + goal.theta ) + goal.x;
00161 ry = -reference.ranges[i] * cos( (i * reference.angle_increment) + reference.angle_min + goal.theta) + goal.y;
00162
00163 for (unsigned int j = 0 ; j < scan.ranges.size(); j++) {
00164 if (scan.ranges[j] < 7.9){
00165 sx = scan.ranges[j] * sin( (j * scan.angle_increment) + scan.angle_min);
00166 sy = -scan.ranges[j] * cos( (j * scan.angle_increment) + scan.angle_min);
00167 distance = sqrt((rx-sx)*(rx-sx) + (ry-sy)*(ry-sy));
00168 if (distance < min) {
00169 min = distance;
00170 }
00171 }
00172 }
00173 error += min;
00174 count++;
00175 }
00176 }
00177 return error / count;
00178 }
00179
00180