Homepage Demos Overview Downloads Tutorials 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   int startrec,stoprec;
00016   SharedObject<MotionSequenceMC<MotionSequence::SizeSmall> > pan;
00017   pan->setPlayTime(startrec=reposTime);
00018   pan->setPose(PostureEngine(config->motion.makePath("stand.pos").c_str()));
00019   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00020   pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MaxRange]);
00021   pan->setOutputCmd(HeadOffset+RollOffset,0);
00022   pan->setPlayTime(stoprec=pan->getPlayTime()+panTime);
00023   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00024   pan->setOutputCmd(HeadOffset+PanOffset,outputRanges[HeadOffset+PanOffset][MinRange]);
00025   pan->setOutputCmd(HeadOffset+RollOffset,0);
00026   pan->setPlayTime(pan->getPlayTime()+reposTime);
00027   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00028   pan->setOutputCmd(HeadOffset+PanOffset,0);
00029   pan->setOutputCmd(HeadOffset+RollOffset,0);
00030   pan->setPlayTime(pan->getPlayTime()+reposTime);
00031   pan->setOutputCmd(HeadOffset+TiltOffset,0);
00032   pan->setOutputCmd(HeadOffset+PanOffset,0);
00033   pan->setOutputCmd(HeadOffset+RollOffset,0);
00034   motman->addPrunableMotion(pan);
00035   erouter->addTimer(this,0,startrec+lagTime,false);
00036   erouter->addTimer(this,1,stoprec+lagTime,false);
00037 }
00038 
00039 void
00040 WallTestBehavior::DoStop() {
00041   erouter->removeListener(this);
00042   BehaviorBase::DoStop(); // do this last
00043 }
00044 
00045 void
00046 WallTestBehavior::processEvent(const EventBase& e) {
00047   if(e.getGeneratorID()==EventBase::sensorEGID) {
00048 #ifdef TGT_ERS7
00049     float nd = state->sensors[NearIRDistOffset];
00050     if(false && nd<350) //force always use the far sensor - near is crappy(ier); without the 'false', would use either one
00051       usedNear.push_back(true);
00052     else {
00053       nd=state->sensors[FarIRDistOffset];
00054       usedNear.push_back(false);
00055     }
00056 #else //not TGT_ERS7
00057     float nd = state->sensors[IRDistOffset];
00058 #endif //not TGT_ERS7
00059     float na = state->outputs[HeadOffset+PanOffset];
00060     //cout << nd << ' ' << na << endl;
00061     d.push_back(nd);
00062     a.push_back(na);
00063 
00064   } else if(e.getSourceID()==0) {
00065     erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID);
00066   } else if(e.getSourceID()==1) {
00067     erouter->removeListener(this,EventBase::sensorEGID);
00068         
00069     PostureEngine pose;
00070     pose.clear();
00071     //float legheight=NEWMAT::ColumnVector(pose.getFrameInterestPoint(BaseFrameOffset,"LFrPaw"))(3);
00072 
00073     cout << "Logging Non-Kinematic calculations to /data/raw_xy.txt" << endl;
00074     {
00075       ofstream rawxy("/ms/data/raw_xy.txt");
00076       if(!rawxy) {
00077         cout << "Could not open file" << endl;
00078       } else {
00079         cout << "Columns are:\tx\ty" << endl;
00080         for(unsigned int i=0; i<d.size(); i++)
00081           rawxy << d[i]*cos(a[i]) << '\t' << d[i]*sin(a[i]) << '\n';
00082       }
00083     }
00084 
00085     cout << "Logging uncalibrated kinematic calculations to /data/k_xyz.txt" << endl;
00086     //assumes IR is parallel to camera
00087     {
00088       ofstream kxys("/ms/data/k_xyz.txt");
00089       if(!kxys) {
00090         cout << "Could not open file" << endl;
00091       } else {
00092 #ifdef TGT_ERS7
00093         cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00094         float off=NEWMAT::ColumnVector(pose.getFrameInterestPoint(CameraFrameOffset,"NearIR"))(3);
00095 #else //not TGT_ERS7
00096         cout << "Columns are:\tx\ty\tz" << endl;
00097         float off=NEWMAT::ColumnVector(pose.getFrameInterestPoint(CameraFrameOffset,"IR"))(3);
00098 #endif //not TGT_ERS7
00099         for(unsigned int i=0; i<d.size(); i++) {
00100           pose(HeadOffset+PanOffset)=a[i];
00101           NEWMAT::ColumnVector hit=pose.frameToBase(CameraFrameOffset)*Kinematics::pack(0,0,d[i]+off);
00102 #ifdef TGT_ERS7
00103           kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00104 #else //not TGT_ERS7
00105           kxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00106 #endif //not TGT_ERS7
00107         }
00108       }
00109     }
00110 
00111     cout << "Logging calibrated kinematic calculations to /data/ck_xyz.txt" << endl;
00112     {
00113       ofstream ckxys("/ms/data/ck_xyz.txt");
00114       if(!ckxys) {
00115         cout << "Could not open file" << endl;
00116       } else {
00117 #ifdef TGT_ERS7
00118         cout << "Columns are:\tx\ty\tz\tis_using_near" << endl;
00119 #else //not TGT_ERS7
00120         cout << "Columns are:\tx\ty\tz" << endl;
00121 #endif //not TGT_ERS7
00122         for(unsigned int i=0; i<d.size(); i++) {
00123           pose(HeadOffset+PanOffset)=a[i];
00124 #ifdef TGT_ERS7
00125           unsigned int frame=usedNear[i]?NearIRFrameOffset:FarIRFrameOffset;
00126           NEWMAT::ColumnVector hit=pose.frameToBase(frame)*Kinematics::pack(0,0,d[i]);
00127           ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\t' << usedNear[i] << '\n';
00128 #else //not TGT_ERS7
00129           NEWMAT::ColumnVector hit=pose.frameToBase(IRFrameOffset)*Kinematics::pack(0,0,d[i]);
00130           ckxys << hit(1) << '\t' << hit(2) << '\t' << hit(3) << '\n';
00131 #endif //not TGT_ERS7
00132         }
00133       }
00134     }
00135 
00136     //find data regions
00137     unsigned int start[3];
00138     unsigned int stop[3];
00139     start[0]=stop[0]=0;
00140     while(a[stop[0]++]>60*M_PI/180 && stop[0]<a.size()) ;
00141     start[1]=stop[0];
00142     while(a[start[1]++]>30*M_PI/180 && start[1]<a.size()) ;
00143     stop[1]=start[1];
00144     while(a[stop[1]++]>-30*M_PI/180 && stop[1]<a.size()) ;
00145     start[2]=stop[1];
00146     while(a[start[2]++]>-60*M_PI/180 && start[2]<a.size()) ;
00147     stop[2]=a.size();
00148       
00149     cout << "Regions are: ";
00150     for(int i=0; i<3; i++)
00151       cout << start[i] << "-" << stop[i] << (i==2?"":", ");
00152     cout << endl;
00153 
00154     for(int w=0; w<3; w++) {
00155       cout << "Wall "<<w<<": ";
00156       //convert radial coordinates to x,y coordinates
00157       int n=stop[w]-start[w];
00158       if(n<3) {
00159         cout << "not enough data - did you forget to turn off estop?" << endl;
00160         continue;
00161       }
00162       vector<float> x,y;
00163       for(unsigned int i=start[w]; i<stop[w]; i++) {
00164 #ifdef TGT_ERS7
00165         if(d[i]==200 || d[i]>1500) //limits for the far sensor, near sensor is [50,500]
00166           continue;
00167 #else //not ers-7
00168         if(d[i]==100 || d[i]>700)
00169           continue;
00170 #endif //not ers-7
00171         x.push_back(d[i]*cos(a[i]));
00172         y.push_back(d[i]*sin(a[i]));
00173       }
00174       cout << "(" << x.size() << " valid samples)" << endl;
00175       if(x.size()<3) {
00176         cout << "No wall" << endl;
00177         continue;
00178       }
00179 
00180       float x0=0,x1=0;
00181       TimeET t;
00182 
00183       t.Set();
00184       solve1(x,y,x0,x1);
00185       cout << "   QR: 'y = "<<x0<<"x + "<<x1<<"'\tTime:"<<t.Age()<<endl;
00186 
00187       t.Set();
00188       solve2(x,y,x0,x1);
00189       cout << "  SVD: '"<<x0<<"x + "<<x1<<"y = 1'\tTime:"<<t.Age()<<endl;
00190     }
00191   }
00192 }
00193 
00194 void
00195 WallTestBehavior::solve1(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00196   NEWMAT::Matrix A(x.size(),2); //x values in first column, 1's in second column (homogenous coordinates)
00197   NEWMAT::ColumnVector b(x.size()); //vector of y values
00198   for(unsigned int i=0; i<x.size(); i++) {
00199     A(i+1,1)=x[i];
00200     A(i+1,2)=1;
00201     b(i+1)=y[i];
00202   }
00203 
00204   NEWMAT::UpperTriangularMatrix U;
00205   NEWMAT::Matrix M;
00206 
00207   NEWMAT::QRZ(A,U);
00208   NEWMAT::QRZ(A,b,M);
00209   NEWMAT::ColumnVector dq = U.i()*M;
00210 
00211   x0=dq(1);
00212   x1=dq(2);
00213 }
00214 
00215 void
00216 WallTestBehavior::solve2(const std::vector<float>& x, const std::vector<float>& y, float& x0, float& x1) {
00217   NEWMAT::Matrix A(x.size(),3); //x values in first column, y's in the second, -1's in third column (homogenous coordinates)
00218   for(unsigned int i=0; i<x.size(); i++) {
00219     A(i+1,1)=x[i];
00220     A(i+1,2)=y[i];
00221     A(i+1,3)=-1;
00222   }
00223   
00224   NEWMAT::DiagonalMatrix Q;
00225   NEWMAT::Matrix U,V;
00226   
00227   NEWMAT::SVD(A,Q,U,V,false,true);
00228   
00229   x0=V(1,V.ncols())/V(3,V.ncols());
00230   x1=V(2,V.ncols())/V(3,V.ncols());
00231 }

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:41 2004 by Doxygen 1.3.9.1