$search
00001 #include <stdio.h> 00002 #include <art_map/Matrix.h> 00003 #include <art_map/FilteredPolygon.h> 00004 00005 FilteredPolygon::FilteredPolygon() 00006 { 00007 // Set up the initial KF matrix entries for each point 00008 int numStates=2; 00009 00010 Matrix initStates = Matrix(numStates,1,false); 00011 initStates[0][0] = -0.001; // Initially place robot at basically 0,0 00012 initStates[1][0] = -0.001; 00013 00014 Matrix uncert = Matrix(numStates,numStates,false); 00015 uncert[0][0] = 6.25; // Standard deviation is 5.0 metres 00016 uncert[1][1] = 6.25; 00017 00018 // Start the KF for each point 00019 for (int i=0; i<NUM_POINTS; i++) { 00020 point[i].Start(numStates,uncert,initStates); 00021 point[i].active=true; // Turn the KF on .. supports multiple models which we don't need here 00022 } 00023 00024 // Set the KF update parameters that don't change between vision frames 00025 distStruct.rejectOutliers = false; 00026 distStruct.outlierSD = 10.0; 00027 distStruct.mainFilterAngleUpdate = false; 00028 distStruct.ingoreLongRangeUpdate = false; 00029 distStruct.deadzoneSize = 0.25; // 25cm 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 // Sets the location of the filtered point ... 00048 // 00049 // I would suggest only using this to set the original location 00050 // because it changes the X matrix directly and therefore changing it 00051 // other times could corrupt the relationship between X and P 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 // Do an update on a point given vision input (distance and angle) 00064 // *TODO* -> rX,rY,rOri are the location of the robot ... this needs to be fixed 00065 // Also tune ! 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 // The current state of the Kalman Filter 00076 Matrix X = point[pointID].GetStates(); 00077 00078 float visionElevation=0; 00079 float dist = visionDistance*cos(visionElevation); 00080 float angle = visionBearing; 00081 00082 // ---- Distance Update 00083 float Rdist = dist*dist/50 ; // modified .. *TODO* Tune this number 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 // ---- Angle Update 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 // printf("%lf %lf\n",estAngle,visionBearing); 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 // Jacobian for Distance and Angle, pass in the location of the robot 00127 // and then the current x,y of the point. Returns a matrix .. 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 }