00001 #include <stdio.h>
00002 #include <art_map/Matrix.h>
00003 #include <art_map/FilteredPolygon.h>
00004
00005 FilteredPolygon::FilteredPolygon()
00006 {
00007
00008 int numStates=2;
00009
00010 Matrix initStates = Matrix(numStates,1,false);
00011 initStates[0][0] = -0.001;
00012 initStates[1][0] = -0.001;
00013
00014 Matrix uncert = Matrix(numStates,numStates,false);
00015 uncert[0][0] = 6.25;
00016 uncert[1][1] = 6.25;
00017
00018
00019 for (int i=0; i<NUM_POINTS; i++) {
00020 point[i].Start(numStates,uncert,initStates);
00021 point[i].active=true;
00022 }
00023
00024
00025 distStruct.rejectOutliers = false;
00026 distStruct.outlierSD = 10.0;
00027 distStruct.mainFilterAngleUpdate = false;
00028 distStruct.ingoreLongRangeUpdate = false;
00029 distStruct.deadzoneSize = 0.25;
00030 distStruct.ambigObject = false;
00031 distStruct.changeAlpha = true;
00032
00033 angleStruct.rejectOutliers = true;
00034 angleStruct.outlierSD = DEG_T_RAD*10.0;
00035 angleStruct.mainFilterAngleUpdate = true;
00036 angleStruct.ingoreLongRangeUpdate = false;
00037 angleStruct.deadzoneSize = DEG_T_RAD*2;
00038 angleStruct.ambigObject = false;
00039 angleStruct.changeAlpha = true;
00040
00041
00042 #ifdef DEBUGFILTER
00043 printf("Constructed FilteredPolygon\n");
00044 #endif
00045 }
00046
00047
00048
00049
00050
00051
00052 void FilteredPolygon::SetPoint(int pointID, float x, float y) {
00053 Matrix X=point[pointID].GetStates();
00054 X[0][0]=x;
00055 X[1][0]=y;
00056 point[pointID].SetStates(X);
00057
00058 #ifdef DEBUGFILTER
00059 printf("Point %i set to (%f,%f)\n",pointID,x,y);
00060 #endif
00061 }
00062
00063
00064
00065
00066 void FilteredPolygon::UpdatePoint(int pointID, float visionDistance,
00067 float visionBearing, float confidence,
00068 float rX, float rY, float rOri)
00069 {
00070 #ifdef DEBUGFILTER
00071 Matrix X2=point[pointID].GetStates();
00072 printf("(%f,%f)->",X2[0][0],X2[1][0]);
00073 #endif
00074
00075
00076 Matrix X = point[pointID].GetStates();
00077
00078 float visionElevation=0;
00079 float dist = visionDistance*cos(visionElevation);
00080 float angle = visionBearing;
00081
00082
00083 float Rdist = dist*dist/50 ;
00084 Matrix Cdist = GetDistanceJacobian(rX, rY, X[0][0], X[1][0]);
00085 float estDist = sqrt(SQUARE(rX - X[0][0]) + SQUARE(rY - X[1][0]));
00086
00087 distStruct.R=Rdist;
00088 distStruct.Y=ABS(dist);
00089 distStruct.Ybar=estDist;
00090 distStruct.dist=dist;
00091 int updateSuccessD = point[pointID].MeasurementUpdateExtended(Cdist,distStruct);
00092
00093
00094
00095 float Rangle = 0.002*10;
00096 Matrix Cangle = GetAngleJacobian(rX, rY, X[0][0], X[1][0]);
00097 float estAngle = Normalise_PI(atan2(rY-X[1][0],rX-X[0][0]) - rOri);
00098
00099 angleStruct.R=Rangle;
00100 angleStruct.Y=angle;
00101 angleStruct.Ybar=estAngle;
00102 angleStruct.dist=dist;
00103 int updateSuccessA =
00104 point[pointID].MeasurementUpdateExtended(Cangle,angleStruct);
00105
00106 if (updateSuccessD!=KF_SUCCESS) {
00107 #ifdef DEBUGFILTER
00108 printf("updateSuccessD %i",updateSuccessD);
00109 #endif
00110 }
00111 if (updateSuccessA!=KF_SUCCESS) {
00112 #ifdef DEBUGFILTER
00113 printf("updateSuccessA failed %i",updateSuccessA);
00114 #endif
00115 }
00116
00117 #ifdef DEBUGFILTER
00118 X2=point[pointID].GetStates();
00119 printf("(%f,%f)",X2[0][0],X2[1][0]);
00120 Matrix P2=point[pointID].GetErrorMatrix();
00121 printf("(%f,%f)\n",P2[0][0],P2[1][0]);
00122 #endif
00123 }
00124
00125
00126
00127
00128 Matrix FilteredPolygon::GetDistanceJacobian(float xb, float yb,
00129 float x, float y)
00130 {
00131 float dist = sqrt((x-xb)*(x-xb) + (y-yb)*(y-yb));
00132 if (dist == 0) dist = 0.00001;
00133 Matrix C = Matrix(1,2,false);
00134 C[0][0] = (x-xb)/dist;
00135 C[0][1] = (y-yb)/dist;
00136 return C;
00137 }
00138
00139 Matrix FilteredPolygon::GetAngleJacobian(float xb, float yb, float x, float y)
00140 {
00141 float distSqrd = (x-xb)*(x-xb) + (y-yb)*(y-yb);
00142 if (distSqrd == 0) distSqrd = 0.00001;
00143 Matrix C = Matrix(1,2,false);
00144 C[0][0] = (yb-y)/distSqrd;
00145 C[0][1] = (x-xb)/distSqrd;
00146 return C;
00147 }
00148
00149 void FilteredPolygon::SetPolygon(poly p)
00150 {
00151 polygon_ = p;
00152 SetPoint(0,p.p1.x,p.p1.y);
00153 SetPoint(1,p.p2.x,p.p2.y);
00154 SetPoint(2,p.p3.x,p.p3.y);
00155 SetPoint(3,p.p4.x,p.p4.y);
00156 }
00157
00158 poly FilteredPolygon::GetPolygon()
00159 {
00160 Matrix X=point[0].GetStates();
00161 polygon_.p1 = MapXY(X[0][0],X[1][0]);
00162 X=point[1].GetStates();
00163 polygon_.p2 = MapXY(X[0][0],X[1][0]);
00164 X=point[2].GetStates();
00165 polygon_.p3 = MapXY(X[0][0],X[1][0]);
00166 X=point[3].GetStates();
00167 polygon_.p4 = MapXY(X[0][0],X[1][0]);
00168
00169 polygon_.heading = ops_.PolyHeading(polygon_);
00170 polygon_.midpoint = ops_.centerpoint(polygon_);
00171 polygon_.length = ops_.getLength(polygon_);
00172
00173 return polygon_;
00174 }
00175
00177 art_msgs::ArtQuadrilateral FilteredPolygon::GetQuad()
00178 {
00179 poly p = GetPolygon();
00180 art_msgs::ArtQuadrilateral q;
00181 q.poly.points.resize(art_msgs::ArtQuadrilateral::quad_size);
00182
00183 q.poly.points[art_msgs::ArtQuadrilateral::bottom_left].x = p.p1.x;
00184 q.poly.points[art_msgs::ArtQuadrilateral::bottom_left].y = p.p1.y;
00185 q.poly.points[art_msgs::ArtQuadrilateral::top_left].x = p.p2.x;
00186 q.poly.points[art_msgs::ArtQuadrilateral::top_left].y = p.p2.y;
00187 q.poly.points[art_msgs::ArtQuadrilateral::top_right].x = p.p3.x;
00188 q.poly.points[art_msgs::ArtQuadrilateral::top_right].y = p.p3.y;
00189 q.poly.points[art_msgs::ArtQuadrilateral::bottom_right].x = p.p4.x;
00190 q.poly.points[art_msgs::ArtQuadrilateral::bottom_right].y = p.p4.y;
00191
00192 q.midpoint.x = p.midpoint.x;
00193 q.midpoint.y = p.midpoint.y;
00194
00195 q.heading = p.heading;
00196 q.length = p.length;
00197 q.poly_id = p.poly_id;
00198
00199 q.is_stop = p.is_stop;
00200 q.is_transition = p.is_transition;
00201 q.contains_way = p.contains_way;
00202
00203 q.start_way.seg = p.start_way.seg;
00204 q.start_way.lane = p.start_way.lane;
00205 q.start_way.pt = p.start_way.pt;
00206
00207 q.end_way.seg = p.end_way.seg;
00208 q.end_way.lane = p.end_way.lane;
00209 q.end_way.pt = p.end_way.pt;
00210
00211 #if 0 //TODO
00212 q.left_boundary.lane_marking = p.left_boundary.lane_marking;
00213 q.right_boundary.lane_marking = p.right_boundary.lane_marking;
00214 #endif
00215
00216 return q;
00217 }