Permalink
Browse files

updating location to master

  • Loading branch information...
Andrew Lawrence
Andrew Lawrence committed Apr 17, 2010
2 parents ed080c7 + edc6dfb commit bb836f58861095302c7c524955fb10b60b7dc020
Showing with 3,154 additions and 2,613 deletions.
  1. +8 −1 Makefile
  2. +41 −10 TODO.org
  3. +2 −0 cmake/base_definitions.cmake
  4. +0 −1 include/Common.h
  5. +0 −1 include/debug.h
  6. +0 −66 include/ifdefs.h
  7. +1 −1 noggin/Brain.py
  8. +5 −6 noggin/EKF.h
  9. +0 −1 noggin/FallStates.py
  10. +65 −28 noggin/LocEKF.cpp
  11. +4 −0 noggin/LocEKF.h
  12. +0 −424 noggin/NavStates.py
  13. +0 −213 noggin/Navigator.py
  14. +7 −7 noggin/Noggin.cpp
  15. +1 −1 noggin/NogginConstants.py
  16. +1 −1 noggin/NogginStructs.h
  17. +1 −2 noggin/Observation.cpp
  18. +1 −3 noggin/cmake.noggin/CMakeLists.txt
  19. +41 −0 noggin/headTracking/BasicStates.py
  20. +7 −1 noggin/headTracking/HeadTracking.py
  21. +1 −1 noggin/headTracking/HeadTrackingHelper.py
  22. +12 −4 noggin/headTracking/PanningStates.py
  23. +1 −28 noggin/headTracking/TrackingStates.py
  24. +24 −0 noggin/navigator/ChaseStates.py
  25. +1 −1 noggin/{ → navigator}/NavConstants.py
  26. +140 −0 noggin/navigator/NavHelper.py
  27. +1 −0 noggin/navigator/NavMath.py
  28. +268 −0 noggin/navigator/NavStates.py
  29. +130 −0 noggin/navigator/Navigator.py
  30. +1 −0 noggin/navigator/__init__.py
  31. +4 −4 noggin/offline/fakerIterators.cpp
  32. +16 −11 noggin/players/BrunswickStates.py
  33. +13 −17 noggin/players/ChaseBallStates.py
  34. +4 −7 noggin/players/ChaseBallTransitions.py
  35. +0 −1 noggin/players/DataStates.py
  36. +6 −11 noggin/players/FindBallStates.py
  37. +15 −14 noggin/players/GoaliePositionStates.py
  38. +2 −2 noggin/players/GoalieTransitions.py
  39. +8 −6 noggin/players/KickingConstants.py
  40. +9 −8 noggin/players/KickingHelpers.py
  41. +13 −32 noggin/players/KickingStates.py
  42. +13 −13 noggin/players/MotionStates.py
  43. +24 −22 noggin/players/PositionStates.py
  44. +53 −75 noggin/players/SoccerFSA.py
  45. +2 −2 noggin/players/SquatPositionStates.py
  46. +1 −1 noggin/players/WalkTestStates.py
  47. +15 −25 noggin/players/pBrunswick.py
  48. +3 −7 noggin/typeDefs/Ball.py
  49. +0 −2 noggin/typeDefs/Landmarks.py
  50. +121 −0 noggin/typeDefs/Location.py
  51. +3 −3 noggin/typeDefs/MyInfo.py
  52. +7 −7 noggin/typeDefs/TeamMember.py
  53. +3 −1 noggin/typeDefs/VisualObject.py
  54. +0 −48 noggin/util/MyMath.py
  55. +1 −1 vision/Ball.cpp
  56. +0 −2 vision/Ball.h
  57. +0 −1 vision/Blob.h
  58. +0 −2 vision/Blobs.h
  59. +311 −184 vision/ConcreteCorner.cpp
  60. +65 −59 vision/ConcreteCorner.h
  61. +246 −113 vision/ConcreteLine.cpp
  62. +44 −27 vision/ConcreteLine.h
  63. +3 −3 vision/Cross.cpp
  64. +6 −7 vision/Field.cpp
  65. +890 −836 vision/FieldLines.cpp
  66. +106 −72 vision/FieldLines.h
  67. +0 −2 vision/ObjectFragments.h
  68. +21 −21 vision/PyVision.cpp
  69. +2 −2 vision/PyVision.h
  70. +0 −1 vision/Threshold.cpp
  71. +14 −27 vision/Utility.cpp
  72. +2 −6 vision/Utility.h
  73. +6 −6 vision/Vision.cpp
  74. +1 −1 vision/Vision.h
  75. +0 −1 vision/VisualBall.h
  76. +174 −24 vision/VisualCorner.cpp
  77. +41 −22 vision/VisualCorner.h
  78. +6 −1 vision/VisualCross.cpp
  79. +1 −2 vision/VisualCross.h
  80. +5 −0 vision/VisualFieldObject.cpp
  81. +1 −0 vision/VisualFieldObject.h
  82. +2 −1 vision/VisualLandmark.h
  83. +90 −56 vision/VisualLine.cpp
  84. +12 −13 vision/VisualLine.h
  85. +5 −0 vision/cmake.vision/buildconfig.cmake
View
@@ -116,4 +116,11 @@ clean:
@$(MAKE) -C comm clean
$(RM) -r install/* $(PYC_FILES)
-
+check:
+ @if [ -e $(CROSS_FILE) ]; then \
+ echo "Making for CROSS"; \
+ elif [ -e $(STRAIGHT_FILE) ]; then \
+ echo "Making for STRAIGHT"; \
+ elif [ -e $(WEBOTS_FILE) ]; then \
+ echo "Making for WEBOTS"; \
+ fi
View
@@ -113,25 +113,56 @@
** Vision
*** TODO Opponent recognition (also with sonar)
*** TODO Open goal detection
-*** TODO Improve lines
-
+*** TODO Improve FieldLines
+**** TODO Implement TOOL-side regression testing in learning modul
+**** Corners
+***** DONE Check edge detection
+***** TODO [#A] Improve green between line endpoints and corner sanity check
+***** TODO Improve CC detection
+****** TODO Improve intersection between endpoints detection
+ Possibly: augment simple between(intersection,endpoint1,endpoint2) with info about where the visible line ends, how many points on each side of it.
+****** TODO When detecting a CC, label the line as CC-Lines
+***** TODO Improve T detection
+ Some of CC detection could be applied here
+***** TODO Improve T classification
+****** Use length of bar and stem
+ Bottom/top T corners will have very long stem and bar. Either means it's a bottom/top.
+***** DONE Use some ambiguous landmarks to ID corners
+ A goal post can at least narrow down the choices.
+***** TODO Use other VisualCorners to identify corners
+ If there is an L corner near a T corner, then the T-corner is a goal T corner
+***** TODO Fix CC-unused point identification
+ Sees far too many CC corners because of stray points. Maybe check on opposite side of T-intersection for lots of white/try and create line points on opposite side (maybe in tighter configuration).
+***** TODO Fix/remove field angle sanity check
+ Possibly just broaden it significantly
+***** TODO Set shape when setting possible corners
+***** TODO Loop through and remark all CC-intersections given other corners
+**** Lines
+***** TODO Better use corner information to identify lines
+***** TODO Identify lines without corners
+***** TODO Extend lines?
+ Maybe scan off the end of a line for more white, then try to make more line points, tightly and add them to the line. Debug current system.
** Localization
-*** TODO Improve Kalman filter
-**** Investigate using unscented or multi-modal filter
-**** TODO Improve handling of unexpected observations
+*** TODO [#B] Improve Kalman filter
+**** TODO [#B] Investigate using unscented or multi-modal filter
+**** TODO [#B] Improve handling of unexpected observations
Could turn it off or use a counter for when to use observations again.
-**** TODO Move from boost/UBLAS to the Eigen matrix library.
-*** TODO Do ball covariance differently / landmark cartesian coordinates
+**** TODO [#C] Move from boost/UBLAS to the Eigen matrix library.
+*** DONE Do ball covariance differently / landmark cartesian coordinates
There will be comments in the LocEKF and BallEKF describing this
*** TODO Investigate sensor based odometry
+*** TODO [#B] Test/improve odometry
+ Take a robot with a blank color table, have it walk forward for a set distance. See what the EKF gives and compare it to the truth value.
*** TODO Topological localization
-*** TODO Teammate localization
-*** TODO Opponent localization
+*** TODO [#C] Teammate localization
+*** TODO [#C] Opponent localization
Each opponent can be treated the same way as the ball. The code from the BallEKF should work fine, the one difference is that we need to do data association between the observations and the different robot estimates. Said otherwise, we have to track four opponent robots and we need to have some way of matching the observation to the correct of the four robots. First try is nearest neighbor. The literature has many other solutions. If people need help send Tucker an email and he can give you some jumping off papers.
-*** TODO Better ambiguous landmark usage
+*** TODO [#A] Better ambiguous landmark usage
**** Use a joint probability for all landmarks, instead of a NN association for each landmark independently
**** Use a multi-modal kalman filter
+**** TODO Use L-Corner shape to identify ambiguous corners.
+ If we're not in the goalbox, field corners should look very different than other ones.
** Behaviors
*** TODO [#A] Replace hand coded kick decision with computational best kick choice.
@@ -34,6 +34,8 @@ IF( "x$ENV{AL_DIR}x" STREQUAL "xx")
SET( AL_DIR "/usr/local/nao-1.6" )
ENDIF (WEBOTS_BACKEND)
SET( ENV{AL_DIR} ${AL_DIR} )
+ MESSAGE( STATUS
+ "Environment variable AL_DIR was not set, reseting to default ${AL_DIR}!" )
ELSE( "x$ENV{AL_DIR}x" STREQUAL "xx")
SET( AL_DIR $ENV{AL_DIR} )
ENDIF( "x$ENV{AL_DIR}x" STREQUAL "xx")
View
@@ -6,7 +6,6 @@
#define Common_h_DEFINED
#include <math.h> // for PI
-#include "ifdefs.h"
#include "NBMath.h"
// ROBOT TYPES
View
@@ -1,7 +1,6 @@
#ifndef DEBUG_H_DEFINED
#define DEBUG_H_DEFINED
-#include "ifdefs.h"
#include "Common.h"
#if !defined(OFFLINE) && ROBOT(AIBO)
View
@@ -1,66 +0,0 @@
-
-//
-// ifdefs.h
-// All macros defined in this file are/should be compile-time switches, for
-// ease of use and change on the robot (ex. Compiling for different robots, or
-// online versus offline). As such, all macros are also required to check for
-// redefinition (i.e. via ifndef) so as to allow command-line specified
-// definitions to take priority.
-//
-
-
-// Offline switch
-#ifndef OFFLINE
-//# define OFFLINE
-#endif
-
-// Cortex 2.0 -- not used
-#if defined(OFFLINE) && !defined(CORTEX__2_0)
-//# define CORTEX__2_0
-#endif
-
-
-//-----System Switches (uncomment the define to activate the system)----//
-// Turn on/off the entire Vision-processing system
-#ifndef USE_VISION
-//# define USE_VISION //js - should be set with cmake now
-#endif
-// it's baaack. due to battery life, here's an easy ifdef to enable/disable
-// Python
-//#ifndef USE_PYTHON
-//# define USE_PYTHON
-//#endif
-// Use the old, small calibration tables
-#ifndef SMALL_TABLES
-//# define SMALL_TABLES
-#endif
-
-#ifndef USE_TOOL_CONNECT
-//# define USE_TOOL_CONNECT
-#endif
-
-// Switches for debugging purposes
-
-
-#ifndef USE_TIME_PROFILING
-//# define USE_TIME_PROFILING
-//# define USE_PROFILER_AUTO_PRINT
-#endif
-#ifndef FRAME_BY_FRAME_PROFILING
-//# define FRAME_BY_FRAME_PROFILING
-#endif
-#ifndef USE_JPEG
-//# define USE_JPEG
-#endif
-#ifndef USE_AES
-//# define USE_AES
-#endif
-#ifndef NEW_LOGGING
-//# define NEW_LOGGING
-#endif
-
-// Other switches
-
-#ifndef USE_FAST_WALK
-# define USE_FAST_WALK
-#endif
View
@@ -15,7 +15,7 @@
from . import GameController
from . import FallController
from .headTracking import HeadTracking
-from . import Navigator
+from .navigator import Navigator
from .util import NaoOutput
from . import NogginConstants as Constants
from .typeDefs import (MyInfo, Ball, Landmarks, Sonar, Packet, Play, TeamMember)
View
@@ -187,8 +187,8 @@ class EKF
// Kalman gain matrix
StateMeasurementMatrix K_k =
boost::numeric::ublas::scalar_matrix<float>(numStates,
- measurementSize,
- 0.0f);
+ measurementSize,
+ 0.0f);
// Observation jacobian
StateMeasurementMatrix H_k =
boost::numeric::ublas::scalar_matrix<float>(measurementSize,
@@ -207,15 +207,14 @@ class EKF
continue;
}
// Calculate the Kalman gain matrix
- StateMeasurementMatrix pTimesHTrans = prod(P_k_bar, trans(H_k));
+ const StateMeasurementMatrix pTimesHTrans = prod(P_k_bar, trans(H_k));
if(measurementSize == 2){
K_k = prod(pTimesHTrans,
NBMath::invert2by2(prod(H_k, pTimesHTrans) + R_k));
}else{
- MeasurementMatrix temp = prod(H_k, pTimesHTrans) + R_k;
- MeasurementMatrix inv =
- NBMath::solve(temp,
+ const MeasurementMatrix inv =
+ NBMath::solve(prod(H_k, pTimesHTrans) + R_k,
boost::numeric::ublas::identity_matrix<float>(
measurementSize));
K_k = prod(pTimesHTrans, inv);
View
@@ -37,7 +37,6 @@ def standup(guard):
#guard.printf("standup angleY is "+str(inertial.angleY))
if guard.firstFrame():
- guard.brain.tracker.stopHeadMoves()
guard.brain.tracker.setNeutralHead()
# If on back, perform back stand up
View
@@ -239,24 +239,32 @@ void LocEKF::incorporateMeasurement(Observation z,
cout << "\t\t\tIncorporating measurement " << z << endl;
#endif
int obsIndex;
+
+ // Hack in here for if the vision system cannot identify this observation
+ // (no possible identities)
+ // @todo this should never happen, so fix vision system to stop it from happening.
+ if (z.getNumPossibilities() == 0){
+ R_k(0,0) = DONT_PROCESS_KEY;
+ return;
+ }
+
// Get the best fit for ambigious data
// NOTE: this is only done, if useAmbiguous is turned to true
- if (z.getNumPossibilities() > 1) {
+ else if (z.getNumPossibilities() > 1) {
obsIndex = findBestLandmark(&z);
- // No landmark is close enough, don't attempt to use one
- if (obsIndex < 0) {
- R_k(0,0) = DONT_PROCESS_KEY;
- return;
- }
} else {
obsIndex = 0;
}
+ // No landmark is close enough, don't attempt to use one
+ if (obsIndex < 0) {
+ R_k(0,0) = DONT_PROCESS_KEY;
+ return;
+ }
+
if ( z.isLine() ){
incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
- }
-
- else if (z.getVisDistance() < USE_CARTESIAN_DIST) {
+ } else if (z.getVisDistance() < USE_CARTESIAN_DIST) {
incorporateCartesianMeasurement( obsIndex, z, H_k, R_k, V_k);
} else {
incorporatePolarMeasurement( obsIndex, z, H_k, R_k, V_k);
@@ -374,23 +382,28 @@ void LocEKF::incorporatePolarMeasurement(int obsIndex,
const float h_r = xhat_k_bar(2);
pair<float, float> closestLandmarkXY =
- Utility::findClosestLinePointCartesian(l, x_r, y_r, h_r);
+ findClosestLinePointCartesian(l, x_r, y_r, h_r);
// Relativize the closest point
const float relX_p = closestLandmarkXY.first;
const float relY_p = closestLandmarkXY.second;
- if (relX_p == 0.0 && relY_p == 0.0)
+ // If we're standing on top of the point (can happen in rare cases
+ // such as when loc is initialized), don't process it (leads to nan)
+ if (z.getVisDistance() < 0.001 ||
+ (relX_p < 0.001 && relY_p < 0.001)) {
+ R_k(0,0) = DONT_PROCESS_KEY;
return;
+ }
// Jacobians for line updates
- H_k(0,0) = (-2*y_b*x_l*y_l + x_b*(-1 + pow(x_l,2) + pow(y_l,2)) - (-1 + pow(x_l,2) + pow(y_l,2))*x_r)/ (pow(x_b - x_r + x_l*(x_l*(-x_b + x_r) + y_l*(y_b + y_r)),2) + pow(y_b - y_r + y_l*(x_l*(-x_b + x_r) + y_l*(y_b + y_r)),2));
- H_k(0,1) = (2*x_l*y_l*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2*(-1 + pow(y_l,2))* (y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2)));
- H_k(0,2) = 0;
+ H_k(0,0) = (2.0f*(-1.0f + pow(x_l,2.0f))*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2.0f*x_l*y_l*(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.0f*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(0,1) = (2.0f*x_l*y_l*(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)) + 2.0f*(-1.0f + pow(y_l,2.0f))* (y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r)))/ (2.0f*sqrt(pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(0,2) = 0.0f;
- H_k(1,0) = -(((-1 + pow(x_l,2) + pow(y_l,2))* (y_b - y_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2)));
- H_k(1,1) = ((-1 + pow(x_l,2) + pow(y_l,2))*(x_b - x_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2));
- H_k(1,2) = -1;
+ H_k(1,0) = -(((-1.0f + pow(x_l,2.0f) + pow(y_l,2.0f))*(y_b - y_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r), 2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f)));
+ H_k(1,1) = ((-1.0f + pow(x_l,2.0f) + pow(y_l,2.0f))*(x_b - x_r))/ (pow(x_b - x_r + x_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f) + pow(y_b - y_r + y_l*(-(x_b*x_l) - y_b*y_l + x_l*x_r + y_l*y_r),2.0f));
+ H_k(1,2) = -1.0f;
MeasurementVector d_x(2);
d_x(0) = static_cast<float>(hypot(relX_p, relY_p));
@@ -411,8 +424,9 @@ void LocEKF::incorporatePolarMeasurement(int obsIndex,
z_x(1) = z.getVisBearing();
// Get expected values of the post
- const float x_b = z.getPointPossibilities()[obsIndex].x;
- const float y_b = z.getPointPossibilities()[obsIndex].y;
+ PointLandmark bestPossibility = z.getPointPossibilities()[obsIndex];
+ const float x_b = bestPossibility.x;
+ const float y_b = bestPossibility.y;
MeasurementVector d_x(2);
const float x = xhat_k_bar(0);
@@ -478,7 +492,7 @@ int LocEKF::findMostLikelyLine(Observation *z)
vector<LineLandmark> possibleLines = z->getLinePossibilities();
int minIndex = -1;
- float minDivergence = 1000.0f;
+ float minDivergence = 800000.0f;
for (unsigned int i = 0; i < possibleLines.size(); ++i) {
float distance = getMahalanobisDistance(z, possibleLines[i]);
@@ -500,25 +514,26 @@ int LocEKF::findMostLikelyLine(Observation *z)
*/
float LocEKF::getMahalanobisDistance(Observation *z, LineLandmark l)
{
- // x,y coordinates of observed line
- MeasurementVector z_x(2);
- z_x(0) = z->getVisDistance() * cos(z->getVisBearing());
- z_x(1) = z->getVisDistance() * sin(z->getVisBearing());
-
+ // Robot's current estimated position
const float x_r = xhat_k_bar(0);
const float y_r = xhat_k_bar(1);
const float h_r = xhat_k_bar(2);
+
+ // x,y coordinates of observed line in the field frame of reference
+ MeasurementVector z_x(2);
+ z_x(0) = x_r + z->getVisDistance() * cos(z->getVisBearing() + h_r);
+ z_x(1) = y_r + z->getVisDistance() * sin(z->getVisBearing() + h_r);
+
pair<float, float> line_xy =
- Utility::findClosestLinePointCartesian(l, x_r, y_r, h_r);
+ findClosestLinePointCartesian(l, x_r, y_r, h_r);
MeasurementVector u(2);
u(0) = line_xy.first;
u(1) = line_xy.second;
const float dist_sd_2 = pow(z->getDistanceSD(), 2);
- const float v = dist_sd_2 * sin(z->getBearingSD()) * cos(z->getBearingSD());
- const float bsd = z->getBearingSD();
+ const float bsd = z->getVisBearing();
float cosb, sinb;
sincosf(bsd, &sinb, &cosb);
@@ -737,3 +752,25 @@ void LocEKF::deadzone(float &R, float &innovation,
}
}
+// Finds the closest point on a line to the robot's position.
+// Returns relative coordinates of the point, in the frame of
+// reference of the field's coordinate system.
+std::pair<float,float>
+LocEKF::findClosestLinePointCartesian(LineLandmark l, float x_r,
+ float y_r, float h_r)
+{
+ const float x_l = l.dx;
+ const float y_l = l.dy;
+
+ const float x_b = l.x1;
+ const float y_b = l.y1;
+
+ // Find closest point on the line to the robot (global frame)
+ const float x_p = ((x_r - x_b)*x_l + (y_r - y_b)*y_l)*x_l + x_b;
+ const float y_p = ((x_r - x_b)*x_l + (y_r - y_b)*y_l)*y_l + y_b;
+
+ // Relativize the closest point
+ const float relX_p = x_p - x_r;
+ const float relY_p = y_p - y_r;
+ return std::pair<float,float>(relX_p, relY_p);
+}
View
@@ -170,6 +170,10 @@ class LocEKF : public EKF<Observation,
void limitPosteriorUncert();
void clipRobotPose();
void deadzone(float &R, float &innovation, float CPC, float EPS);
+ std::pair<float, float> findClosestLinePointCartesian(LineLandmark l,
+ float x_r,
+ float y_r,
+ float h_r);
// Last odometry update
MotionModel lastOdo;
Oops, something went wrong.

0 comments on commit bb836f5

Please sign in to comment.