FilteredPolygon.cc
Go to the documentation of this file.
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 }


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34