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"
00017 #include "VRmixin.h"
00018
00019 using namespace std;
00020
00021 namespace DualCoding {
00022
00023
00024
00025 VRmixin* VRmixin::theOne=NULL;
00026
00027
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
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;
00088
00089 unsigned int VRmixin::instanceCount = 0;
00090 unsigned int VRmixin::crewCount = 0;
00091
00092 VRmixin::VRmixin() {
00093 if(instanceCount++==0) {
00094
00095
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
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
00173 camShS.clear();
00174 camSkS.bumpRefreshCounter();
00175 camSkS.clear();
00176
00177 localShS.clear();
00178 localSkS.bumpRefreshCounter();
00179 localSkS.clear();
00180
00181 theAgent = Shape<AgentData>();
00182 worldShS.clear();
00183 worldSkS.bumpRefreshCounter();
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
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
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
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
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;
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
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
00328
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 }