Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WallTestBehavior.cc

Go to the documentation of this file.
00001 #include "WallTestBehavior.h"
00002 #include "Shared/newmat/newmat.h"
00003 #include "Shared/newmat/newmatap.h"
00004 #include "Events/EventRouter.h"
00005 #include "Motion/MotionManager.h"
00006 #include "Motion/MotionSequenceMC.h"
00007 #include "Shared/Config.h"
00008 #include "Shared/WorldState.h"
00009 #include "Shared/newmat/newmatio.h"
00010 #include <fstream>
00011 
00012 void
00013 WallTestBehavior::DoStart() {
00014   BehaviorBase::DoStart(); // do this first
00015   
00016   // We'll set these two variables to the time at which we should
00017   // start and stop recording ir values
00018   int startrec,stoprec;
00019   
00020   // This motion sequence will rotate the head left, then right, then back to the middle.
00021   SharedObject<SmallMotionSequenceMC> pan;
00022   // stand up, and move the head to the left, taking `reposTime` to do it
00023   pan->setTime(startrec=reposTime); // reposTime is defined in the header file
00024   pan->setPose(PostureEngine(config->motion.makePath("stand.pos").c_str()));
00025   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00026   pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MaxRange]);
00027   pan->setOutputCmd(HeadOffset+RollOffset,0);
00028   // now pan to the right, taking `panTime`
00029   stoprec=pan->advanceTime(panTime);
00030   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00031   pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MinRange]);
00032   pan->setOutputCmd(HeadOffset+RollOffset,0);
00033   // center the head
00034   pan->advanceTime(reposTime);
00035   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00036   pan->setOutputCmd(HeadOffset+PanOffset,0);
00037   pan->setOutputCmd(HeadOffset+RollOffset,0);
00038   // this second repetition simply forces the head to stay still for a little longer
00039   pan->advanceTime(reposTime);
00040   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00041   pan->setOutputCmd(HeadOffset+PanOffset,0);
00042   pan->setOutputCmd(HeadOffset+RollOffset,0);
00043   // now we add the motion sequence to the motion manager
00044   // this will cause it to start executing
00045   motman->addPrunableMotion(pan);
00046   
00047   // add timers to start and stop sensor recording at the
00048   // times we stored in startrec and stoprec
00049   erouter->addTimer(this,0,startrec+lagTime,false); // SID==0 start recording
00050   erouter->addTimer(this,1,stoprec+lagTime,false);  // SID==1 stop recording
00051 }
00052 
00053 void
00054 WallTestBehavior::DoStop() {
00055   erouter->removeListener(this);
00056   BehaviorBase::DoStop(); // do this last
00057 }
00058 
00059 void
00060 WallTestBehavior::processEvent(const EventBase& e) {
00061   if(e.getGeneratorID()==EventBase::sensorEGID) {
00062     // The ERS-7 has different IR sensors than the 2xx series
00063     // So we need to do a bit of different code depending on the target model
00064 #ifdef TGT_ERS7
00065     float nd = state->sensors[NearIRDistOffset];
00066     /* This code tried to be smart by picking between near or far
00067      * However, without additional calibration work, the two are
00068      * inconsistent.  Current calibration handles direction of the
00069      * beam, but we don't (yet) have any calibration of measurement
00070      * error due to sensor reading long (or short).
00071      * Instead, the following 'false' causes only the 'far' sensor
00072      * to be used */
00073     if(false && nd<350)
00074       usedNear.push_back(true);
00075     else {
00076       nd=state->sensors[FarIRDistOffset];
00077       usedNear.push_back(false);
00078     }
00079 #else //not TGT_ERS7
00080     float nd = state->sensors[IRDistOffset];
00081 #endif //not TGT_ERS7
00082     float na = state->outputs[HeadOffset+PanOffset];
00083     //cout << nd << ' ' << na << endl;
00084     // Just store the measurements for later analysis
00085     d.push_back(nd);
00086     a.push_back(na);
00087 
00088   } else if(e.getSourceID()==0) {
00089     // must be a timer event -- SID 0 indicates start of recording
00090     // so start subscribing to sensor events
00091     erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID);
00092 
00093   } else if(e.getSourceID()==1) {
00094     // must be a timer event -- SID 1 indicates stop of recording
00095     // so stop subscribing to sensor events
00096     erouter->removeListener(this,EventBase::sensorEGID);
00097         
00098     PostureEngine pose;
00099     pose.clear();
00100     //float legheight=NEWMAT::ColumnVector(pose.getJointInterestPoint(BaseFrameOffset,"LFrPaw"))(3);
00101 
00102     /* Extremely simplistic estimation.
00103      * Assumes all readings are relative to the center of head rotation */
00104     cout << "Logging Non-Kinematic calculations to /data/raw_xy.txt" << endl;
00105     {
00106       ofstream rawxy(config->portPath("data/raw_xy.txt").c_str());
00107       if(!rawxy) {
00108         cout << "Could not open file" << endl;
00109       } else {
00110         cout << "Columns are:\tx\ty" << endl;
00111         for(unsigned int i=0; i<d.size(); i++)
00112           rawxy << d[i]*cos(a[i]) << '\t' << d[i]*sin(a[i]) << '\n';
00113       }
00114     }
00115 
00116     /* Less simplistic estimation.
00117      * Uses interest point information to determine location of
00118      * IR sensor from center of rotation, but doesn't make use
00119      * of information regarding direction of the beam */
00120     cout << "Logging uncalibrated kinematic calculations to /data/k_xyz.txt" << endl;
00121     {
00122       ofstream kxys(config->portPath("data/k_xyz.txt").c_str());
00123       if(!kxys) {
00124         cout << "Could not open file" << endl;
00125       } else {
00126         // `off` will store the offset of the sensor from center of rotation
00127 #ifdef TGT_ERS7
00128         cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00129         float off=NEWMAT::ColumnVector(pose.getJointInterestPoint(CameraFrameOffset,"NearIR"))(3);
00130 #else //not TGT_ERS7
00131         cout << "Columns are:\tx\ty\tz" << endl;
00132         float off=NEWMAT::ColumnVector(pose.getJointInterestPoint(CameraFrameOffset,"IR"))(3);
00133 #endif //TGT_ERS7 check
00134         for(unsigned int i=0; i<d.size(); i++) {
00135           pose(HeadOffset+PanOffset)=a[i];
00136           NEWMAT::ColumnVector hit=pose.jointToBase(CameraFrameOffset)*Kinematics::pack(0,0,d[i]+off);
00137 #ifdef TGT_ERS7
00138           kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00139 #else //not TGT_ERS7
00140           kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00141 #endif //TGT_ERS7 check
00142         }
00143       }
00144     }
00145 
00146     /* Decent estimation.
00147      * Uses full kinematic information to determine 3D location
00148      * of the point in space being measured.  These measurements
00149      * can be then projected into the ground plane to get a more
00150      * accurate estimation from the walls.
00151      * However, still doesn't take into account any sensor error,
00152      * such as offsets or scaling issues. (unless such calibration
00153      * has been added into WorldState itself after the time of this
00154      * writing...) */
00155     cout << "Logging calibrated kinematic calculations to /data/ck_xyz.txt" << endl;
00156     {
00157       ofstream ckxys(config->portPath("data/ck_xyz.txt").c_str());
00158       if(!ckxys) {
00159         cout << "Could not open file" << endl;
00160       } else {
00161 #ifdef TGT_ERS7
00162         cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00163 #else //not TGT_ERS7
00164         cout << "Columns are:\tx\ty\tz" << endl;
00165 #endif //not TGT_ERS7
00166         for(unsigned int i=0; i<d.size(); i++) {
00167           pose(HeadOffset+PanOffset)=a[i];
00168 #ifdef TGT_ERS7
00169           unsigned int frame=usedNear[i]?NearIRFrameOffset:FarIRFrameOffset;
00170           NEWMAT::ColumnVector hit=pose.jointToBase(frame)*Kinematics::pack(0,0,d[i]);
00171           ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00172 #else //not TGT_ERS7
00173           NEWMAT::ColumnVector hit=pose.jointToBase(IRFrameOffset)*Kinematics::pack(0,0,d[i]);
00174           ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00175 #endif //not TGT_ERS7
00176         }
00177       }
00178     }
00179 
00180     //find data regions
00181     // This sections the angle of the head into left, forward,
00182     // and right regions, which assumes the walls will be roughly
00183     // orthogonal
00184     unsigned int start[3];
00185     unsigned int stop[3];
00186     start[0]=stop[0]=0;
00187     while(a[stop[0]++]>60*M_PI/180 && stop[0]<a.size()) ;
00188     start[1]=stop[0];
00189     while(a[start[1]++]>30*M_PI/180 && start[1]<a.size()) ;
00190     stop[1]=start[1];
00191     while(a[stop[1]++]>-30*M_PI/180 && stop[1]<a.size()) ;
00192     start[2]=stop[1];
00193     while(a[start[2]++]>-60*M_PI/180 && start[2]<a.size()) ;
00194     stop[2]=a.size();
00195       
00196     cout << "Regions are: ";
00197     for(int i=0; i<3; i++)
00198       cout << start[i] << "-" << stop[i] << (i==2?"":", ");
00199     cout << endl;
00200 
00201     //now process each of those regions independently
00202     for(int w=0; w<3; w++) {
00203       cout << "Wall "<<w<<": ";
00204       //convert radial coordinates to x,y coordinates
00205       int n=stop[w]-start[w];
00206       if(n<3) {
00207         cout << "not enough data - did you forget to turn off estop?" << endl;
00208         continue;
00209       }
00210       vector<float> x,y;
00211       for(unsigned int i=start[w]; i<stop[w]; i++) {
00212 #ifdef TGT_ERS7
00213         if(d[i]==200 || d[i]>1500) //limits for the far sensor, near sensor is [50,500]
00214           continue;
00215 #else //not ers-7
00216         if(d[i]==100 || d[i]>700)
00217           continue;
00218 #endif //not ers-7
00219         x.push_back(d[i]*cos(a[i]));
00220         y.push_back(d[i]*sin(a[i]));
00221       }
00222       cout << "(" << x.size() << " valid samples)" << endl;
00223       if(x.size()<3) {
00224         cout << "No wall" << endl;
00225         continue;
00226       }
00227 
00228 
00229       // Now that we've done some setup and sanity checking, do
00230       // the actual linear least squares computation to fit a line
00231       // There's two methods we'll test, one using QR factorization
00232       // and the other using SVD decomposition.
00233       float x0=0,x1=0;
00234       TimeET t;
00235 
00236       t.Set();
00237       solve1(x,y,x0,x1);
00238       cout << "   QR: 'y = "<<x0<<"x + "<<x1<<"'\tTime:"<<t.Age()<<endl;
00239 
00240       t.Set();
00241       solve2(x,y,x0,x1);
00242       cout << "  SVD: '"<<x0<<"x + "<<x1<<"y = 1'\tTime:"<<t.Age()<<endl;
00243     }
00244   }
00245 }
00246 
00247 void
00248 WallTestBehavior::solve1(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00249   NEWMAT::Matrix A(x.size(),2); //x values in first column, 1's in second column (homogenous coordinates)
00250   NEWMAT::ColumnVector b(x.size()); //vector of y values
00251   for(unsigned int i=0; i<x.size(); i++) {
00252     A(i+1,1)=x[i];
00253     A(i+1,2)=1;
00254     b(i+1)=y[i];
00255   }
00256 
00257   NEWMAT::UpperTriangularMatrix U;
00258   NEWMAT::Matrix M;
00259 
00260   NEWMAT::QRZ(A,U);
00261   NEWMAT::QRZ(A,b,M);
00262   NEWMAT::ColumnVector dq = U.i()*M;
00263 
00264   x0=dq(1);
00265   x1=dq(2);
00266 }
00267 
00268 void
00269 WallTestBehavior::solve2(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00270   NEWMAT::Matrix A(x.size(),3); //x values in first column, y's in the second, -1's in third column (homogenous coordinates)
00271   for(unsigned int i=0; i<x.size(); i++) {
00272     A(i+1,1)=x[i];
00273     A(i+1,2)=y[i];
00274     A(i+1,3)=-1;
00275   }
00276   
00277   NEWMAT::DiagonalMatrix Q;
00278   NEWMAT::Matrix U,V;
00279   
00280   NEWMAT::SVD(A,Q,U,V,false,true);
00281   
00282   x0=V(1,V.ncols())/V(3,V.ncols());
00283   x1=V(2,V.ncols())/V(3,V.ncols());
00284 }

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4