Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

VRmixin.cc

Go to the documentation of this file.
00001 #include <sstream>
00002 
00003 #include "Vision/RawCameraGenerator.h"
00004 #include "Vision/RLEGenerator.h"
00005 #include "Vision/RegionGenerator.h"
00006 #include "Vision/SegmentedColorGenerator.h"
00007 
00008 #include "SketchData.h"
00009 #include "ShapeBlob.h"
00010 #include "Sketch.h"
00011 
00012 #include "Lookout.h"
00013 #include "MapBuilder.h"
00014 #include "Pilot.h"
00015 
00016 #include "ViewerConnection.h"  // for port numbers and buffer sizes
00017 #include "VRmixin.h"
00018 
00019 using namespace std;
00020 
00021 namespace DualCoding {
00022 
00023 //----------------------------------------------------------------
00024 
00025 VRmixin* VRmixin::theOne=NULL;
00026 
00027 //! static function allows us to specify intialization order because static within a function isn't created until the function is called
00028 template<ReferenceFrameType_t _refFrameType,int const init_id, size_t const _width, size_t const _height>
00029 static SketchSpace& createStaticSkS(const std::string& _name) {
00030   static SketchSpace SkS(_name,_refFrameType,init_id,_width,_height);
00031   //  cout << _name << " space is constructed\n";
00032   return SkS;
00033 }
00034 
00035 SketchSpace& VRmixin::getCamSkS() {
00036   return createStaticSkS<camcentric,10000,CameraResolutionX,CameraResolutionY>("cam");
00037 }
00038 SketchSpace& VRmixin::getLocalSkS() {
00039   return createStaticSkS<egocentric,20000,WORLD_WIDTH,WORLD_HEIGHT>("local"); 
00040 }
00041 SketchSpace& VRmixin::getWorldSkS() {
00042   return createStaticSkS<allocentric,30000,WORLD_WIDTH,WORLD_HEIGHT>("world");
00043 }
00044 ShapeSpace& VRmixin::getGroundShS() {
00045   static ShapeSpace ShS(&VRmixin::getCamSkS(),90000,"ground",egocentric);
00046   return ShS;
00047 }
00048 
00049 SketchSpace& VRmixin::camSkS=VRmixin::getCamSkS();
00050 ShapeSpace& VRmixin::camShS=VRmixin::getCamSkS().getDualSpace();
00051 
00052 ShapeSpace& VRmixin::groundShS=VRmixin::getGroundShS();
00053 
00054 SketchSpace& VRmixin::localSkS=VRmixin::getLocalSkS();
00055 ShapeSpace& VRmixin::localShS=VRmixin::getLocalSkS().getDualSpace();
00056 
00057 SketchSpace& VRmixin::worldSkS=VRmixin::getWorldSkS();
00058 ShapeSpace& VRmixin::worldShS=VRmixin::getWorldSkS().getDualSpace();
00059 
00060 Shape<AgentData> VRmixin::theAgent;
00061 
00062 Socket *VRmixin::camDialogSock=NULL;
00063 Socket *VRmixin::camRleSock=NULL;
00064 Socket *VRmixin::localDialogSock=NULL;
00065 Socket *VRmixin::localRleSock=NULL;
00066 Socket *VRmixin::worldDialogSock=NULL;
00067 Socket *VRmixin::worldRleSock=NULL;
00068 
00069 MapBuilder& VRmixin::getMapBuilder() {
00070   static MapBuilder mapbuilderInstance;
00071   return mapbuilderInstance;
00072 }
00073 MapBuilder& VRmixin::mapBuilder = VRmixin::getMapBuilder();
00074 
00075 Lookout& VRmixin::getLookout() {
00076   static Lookout lookoutInstance;
00077   return lookoutInstance;
00078 }
00079 Lookout& VRmixin::lookout = VRmixin::getLookout();
00080 
00081 Pilot& VRmixin::getPilot() {
00082   static Pilot pilotInstance;
00083   return pilotInstance;
00084 }
00085 Pilot& VRmixin::pilot = VRmixin::getPilot();
00086 
00087 PFShapeLocalization* VRmixin::particleFilter = NULL; // will be set by startCrew
00088 
00089 unsigned int VRmixin::instanceCount = 0;
00090 unsigned int VRmixin::crewCount = 0;
00091 
00092 VRmixin::VRmixin() {
00093   if(instanceCount++==0) {
00094     // only want to do the following once
00095     //cout << "Initializing VRmixin statics" << endl;
00096     if (theOne != NULL) {
00097       if ( ! mapBuilder.isRetained() )
00098   cerr << "VRmixin statics already constructed!?!?!" << endl;
00099       return;
00100     }
00101     theOne=this;
00102     camSkS.requireIdx();
00103     localSkS.requireIdx();
00104     worldSkS.requireIdx();
00105     
00106     camDialogSock=wireless->socket(Socket::SOCK_STREAM, 1024, DIALOG_BUFFER_SIZE);
00107     camRleSock=wireless->socket(Socket::SOCK_STREAM, 1024, RLE_BUFFER_SIZE);
00108     worldDialogSock=wireless->socket(Socket::SOCK_STREAM, 1024, DIALOG_BUFFER_SIZE);
00109     worldRleSock=wireless->socket(Socket::SOCK_STREAM, 1024, RLE_BUFFER_SIZE);
00110     localDialogSock=wireless->socket(Socket::SOCK_STREAM, 1024, DIALOG_BUFFER_SIZE);
00111     localRleSock=wireless->socket(Socket::SOCK_STREAM, 1024, RLE_BUFFER_SIZE);
00112     
00113     wireless->setReceiver(camDialogSock->sock, &camDialogSockCallback);
00114     wireless->setReceiver(worldDialogSock->sock, &worldDialogSockCallback);
00115     wireless->setReceiver(localDialogSock->sock, &localDialogSockCallback);
00116     
00117     wireless->setDaemon(camDialogSock,   true);
00118     wireless->setDaemon(camRleSock,      true);
00119     wireless->setDaemon(worldDialogSock, true);
00120     wireless->setDaemon(worldRleSock,    true);
00121     wireless->setDaemon(localDialogSock, true);
00122     wireless->setDaemon(localRleSock,    true);
00123     
00124     wireless->listen(camDialogSock,   CAM_DIALOG_PORT);
00125     wireless->listen(camRleSock,      CAM_RLE_PORT);
00126     wireless->listen(worldDialogSock, WORLD_DIALOG_PORT);
00127     wireless->listen(worldRleSock,    WORLD_RLE_PORT);
00128     wireless->listen(localDialogSock, LOCAL_DIALOG_PORT);
00129     wireless->listen(localRleSock,    LOCAL_RLE_PORT);
00130     
00131     camSkS.viewer->setDialogSocket(camDialogSock,     CAM_DIALOG_PORT);
00132     camSkS.viewer->setRleSocket(camRleSock,           CAM_RLE_PORT);
00133     localSkS.viewer->setDialogSocket(localDialogSock, LOCAL_DIALOG_PORT);
00134     localSkS.viewer->setRleSocket(localRleSock,       LOCAL_RLE_PORT);
00135     worldSkS.viewer->setDialogSocket(worldDialogSock, WORLD_DIALOG_PORT);
00136     worldSkS.viewer->setRleSocket(worldRleSock,       WORLD_RLE_PORT);
00137 
00138     theAgent = Shape<AgentData>(worldShS);
00139     theAgent->setColor(rgb(0,0,255));
00140 
00141     mapBuilder.SetAutoDelete(false);
00142     pilot.SetAutoDelete(false);
00143     lookout.SetAutoDelete(false);
00144   }
00145 }
00146 
00147 VRmixin::~VRmixin() {
00148   if(--instanceCount==0) {
00149     if ( mapBuilder.isRetained() ) return;
00150     // cout << "Destructing VRmixin statics" << endl;
00151     if (theOne == NULL) {
00152       cerr << "VRmixin statics already destructed!?!?!" << endl;
00153       return;
00154     }
00155     
00156     wireless->setDaemon(camDialogSock,  false);
00157     wireless->setDaemon(camRleSock,     false);
00158     wireless->setDaemon(localDialogSock,false);
00159     wireless->setDaemon(localRleSock,   false);
00160     wireless->setDaemon(worldDialogSock,false);
00161     wireless->setDaemon(worldRleSock,   false);
00162     
00163     wireless->close(camRleSock->sock);
00164     wireless->close(camDialogSock->sock);
00165     wireless->close(localRleSock->sock);
00166     wireless->close(localDialogSock->sock);
00167     wireless->close(worldRleSock->sock);
00168     wireless->close(worldDialogSock->sock);
00169     
00170     theOne=NULL;
00171     
00172     // clear ShapeSpace first because it may contain rendering links to SketchSpace
00173     camShS.clear();
00174     camSkS.bumpRefreshCounter(); // release visible sketches
00175     camSkS.clear();
00176     
00177     localShS.clear();
00178     localSkS.bumpRefreshCounter(); // release visible sketches
00179     localSkS.clear();
00180     
00181     theAgent = Shape<AgentData>();
00182     worldShS.clear();
00183     worldSkS.bumpRefreshCounter(); // release visible sketches
00184     worldSkS.clear();
00185     
00186     camSkS.freeIndexes();
00187     localSkS.freeIndexes();
00188     worldSkS.freeIndexes();
00189   }
00190 }
00191 
00192 void VRmixin::startCrew() {
00193   if ( crewCount++ == 0 ) {
00194     //cout << "Starting crew" << endl;
00195     mapBuilder.SetAutoDelete(false);
00196     mapBuilder.DoStart();
00197     lookout.SetAutoDelete(false);
00198     lookout.DoStart();
00199     pilot.SetAutoDelete(false);
00200     pilot.DoStart();
00201     if ( particleFilter == NULL )
00202       particleFilter = new PFShapeLocalization(localShS,worldShS);
00203   }
00204 }
00205 
00206 void VRmixin::stopCrew() {
00207   if ( --crewCount == 0 ) {
00208     //cout << "Stopping crew" << endl;
00209     pilot.DoStop();
00210     lookout.DoStop();
00211     mapBuilder.DoStop();
00212     delete particleFilter;
00213     particleFilter = NULL;
00214   }
00215 }
00216 
00217 void VRmixin::projectToGround(const NEWMAT::Matrix& camToBase,
00218             const NEWMAT::ColumnVector& ground_plane) {
00219   groundShS.clear();
00220   groundShS.importShapes(camShS.allShapes());
00221   const vector<ShapeRoot> &groundShapes_vec = groundShS.allShapes();
00222   for(size_t i = 0; i < groundShapes_vec.size(); i++)
00223     groundShapes_vec[i]->projectToGround(camToBase, ground_plane);
00224 }
00225 
00226 int VRmixin::camDialogSockCallback(char *buf, int bytes) {
00227   static std::string incomplete;
00228   dialogCallback(buf, bytes, incomplete, theOne->camSkS, theOne->camShS);
00229   return 0;
00230 }
00231 
00232 int VRmixin::localDialogSockCallback(char *buf, int bytes) {
00233   static std::string incomplete;
00234   dialogCallback(buf, bytes, incomplete, theOne->localSkS, theOne->localShS);
00235   return 0;
00236 }
00237 
00238 int VRmixin::worldDialogSockCallback(char *buf, int bytes) {
00239   static std::string incomplete;
00240   dialogCallback(buf, bytes, incomplete, theOne->worldSkS, theOne->worldShS);
00241   return 0;
00242 }
00243 
00244 void VRmixin::dialogCallback(char* buf, int bytes, std::string& incomplete,
00245            SketchSpace& SkS, ShapeSpace& ShS) {
00246   std::string s(buf,bytes);
00247   while(s.size()>0) {
00248     size_t endline=s.find('\n');
00249     if(endline==std::string::npos) {
00250       incomplete+=s;
00251       return;
00252     }
00253     else {
00254       incomplete+=s.substr(0,endline);
00255       theOne->processSketchRequest(incomplete,SkS,ShS);
00256       incomplete.erase();
00257       s=s.substr(endline+1);
00258     }
00259   }
00260   return;
00261 }
00262 
00263 bool VRmixin::rleEncodeSketch(const SketchDataRoot& image)
00264 {
00265   unsigned int avail = RLE_BUFFER_SIZE-1;
00266   Socket* rleSock = image.getSpace().viewer->getRleSocket();
00267   char* buf=(char*)rleSock->getWriteBuffer(avail);
00268   ASSERTRETVAL(buf!=NULL,"could not get buffer",false);
00269   unsigned int used = image.saveBuffer(buf, avail);
00270   rleSock->write(used);
00271   return true;
00272 }
00273 
00274 //! Import a color-segmented image as a Sketch<uchar>
00275 Sketch<uchar> VRmixin::sketchFromSeg() {
00276   Sketch<uchar> cam(camSkS, "camimage");
00277   cam->setColorMap(segMap);
00278   size_t const npixels = cam->getNumPixels();
00279   cmap_t* seg_image = ProjectInterface::defSegmentedColorGenerator->getImage(CAM_LAYER,CAM_CHANNEL);
00280   for(size_t i = 0; i < npixels; i++)
00281     cam[i] = seg_image[i];
00282   return cam;
00283 }
00284 
00285 //! Import channel n as a Sketch<uchar>
00286 Sketch<uchar> VRmixin::sketchFromChannel(const RawCameraGenerator::channel_id_t chan) {
00287   const RawCameraGenerator::channel_id_t the_chan =
00288     (chan >= 0 && chan < RawCameraGenerator::NUM_CHANNELS) ? chan : RawCameraGenerator::CHAN_Y;
00289   Sketch<uchar> cam(camSkS,"sketchFromChannel");
00290   cam->setColorMap(grayMap);
00291   uchar* campixels = cam->getRawPixels();
00292   int const incr = ProjectInterface::defRawCameraGenerator->getIncrement(CAM_LAYER);
00293   int const skip = ProjectInterface::defRawCameraGenerator->getSkip(CAM_LAYER);
00294   uchar* chan_ptr = ProjectInterface::defRawCameraGenerator->getImage(CAM_LAYER,the_chan);
00295   if(chan_ptr==NULL) {
00296     for (unsigned int row = 0; row < cam->getHeight(); row++)
00297       for (unsigned int col = 0; col < cam->getWidth(); col++)
00298         *campixels++ = 0;
00299   } else {
00300     chan_ptr -= incr;  // back up by one pixel to prepare for loop
00301     for (unsigned int row = 0; row < cam->getHeight(); row++) {
00302       for (unsigned int col = 0; col < cam->getWidth(); col++)
00303         *campixels++ = *(chan_ptr += incr);
00304       chan_ptr += skip;
00305     }
00306   }
00307   return cam;
00308 }
00309 
00310 Sketch<uchar> VRmixin::sketchFromRawY() {
00311   return sketchFromChannel(RawCameraGenerator::CHAN_Y);
00312 }
00313 
00314 
00315 //! Import the results of the region generator as a vector of Shape<BlobData>
00316 vector<Shape<BlobData> >
00317 VRmixin::getBlobsFromRegionGenerator(const color_index color, 
00318              const int minarea,
00319              const BlobData::BlobOrientation_t orient, 
00320              const coordinate_t height,
00321              const int maxblobs) {
00322   vector<Shape<BlobData> > result;
00323   const CMVision::run<uchar> *rle_buffer = reinterpret_cast<const CMVision::run<uchar>*>
00324     (ProjectInterface::defRLEGenerator->getImage(CAM_LAYER,CAM_CHANNEL));
00325   const CMVision::color_class_state* ccs = reinterpret_cast<const CMVision::color_class_state*>
00326     (ProjectInterface::defRegionGenerator->getImage(CAM_LAYER,CAM_CHANNEL));
00327   //  cout << "Color " << color << " name '" << ccs[color].name 
00328   //   << "' has " << ccs[color].num << " regions." << endl;
00329   const rgb rgbvalue = ProjectInterface::getColorRGB(color);
00330   const CMVision::region* list_head = ccs[color].list;
00331   for (int i=0; list_head!=NULL && i<maxblobs && list_head->area >= minarea; list_head = list_head->next, i++) {
00332     BlobData* blobdat = BlobData::new_blob(camShS,*list_head, rle_buffer, orient, height, rgbvalue);
00333     result.push_back(Shape<BlobData>(blobdat));
00334   }
00335   return result;
00336 }
00337 
00338 void VRmixin::processSketchRequest(const std::string &line,
00339            SketchSpace& SkS, 
00340            ShapeSpace& ShS)
00341 {
00342   Socket* dialogSock = SkS.viewer->getDialogSocket();
00343   if(line.compare(0,strlen("size"),"size")==0) {
00344     dialogSock->printf("size begin\n");
00345     dialogSock->printf("width %d\nheight %d\n",int(SkS.getWidth()),int(SkS.getHeight()));
00346     dialogSock->printf("size end\n");
00347   }
00348   else if(line.compare(0,strlen("list"),"list")==0) {
00349     dialogSock->printf("list begin\n");
00350     SkS.viewer->writeBigString(SkS.getTmatForGUI());  
00351     SkS.viewer->writeBigString(SkS.getSketchListForGUI());  
00352     SkS.viewer->writeBigString(ShS.getShapeListForGUI()); 
00353     dialogSock->printf("list end\n");
00354   } else if(line.compare(0,strlen("get"),"get")==0) {
00355     dialogSock->printf("get begin\n");
00356     std::string tempstring = line.substr(strlen("get"),
00357            line.length()-strlen("get"));
00358     std::istringstream ist(tempstring);
00359     int requested_id = -1;
00360     ist >> requested_id;
00361     dialogSock->printf("get read:%d\n",requested_id);
00362     SketchDataRoot* sketchptr=(SkS.retrieveSketch(requested_id));
00363     if(sketchptr != NULL)
00364       rleEncodeSketch(*sketchptr);
00365     dialogSock->printf("get end\n");
00366   } else {
00367     dialogSock->printf("Invalid command\n");
00368   }
00369 }
00370 
00371 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:37 2007 by Doxygen 1.5.4