StereoVisionI.C

00001 #include "StereoVisionI.H"
00002 #include "Component/ModelParam.H"
00003 #include "Component/ModelOptionDef.H"
00004 #include "Raster/DeBayer.H"
00005 #include "SIFT/VisualObject.H"
00006 #include "Image/ShapeOps.H"
00007 
00008 #ifndef STEREO_VISION_C
00009 #define STEREO_VISION_C
00010 
00011 StereoVision::StereoVision(int id, OptionManager& mgr,
00012                            const std::string& descrName,
00013                            const std::string& tagName) :
00014   RobotBrainComponent(mgr, descrName, tagName),
00015   itsOfs(new OutputFrameSeries(mgr)),
00016   itsOfs2(new OutputFrameSeries(mgr)),
00017   itsOfs3(new OutputFrameSeries(mgr)),
00018   itsOfs4(new OutputFrameSeries(mgr)),
00019   itsFrameCount(0),
00020   lastFrameCount(0)
00021 {
00022   addSubComponent(itsOfs);
00023   addSubComponent(itsOfs2);
00024   addSubComponent(itsOfs3);
00025   addSubComponent(itsOfs4);
00026 }
00027 
00028 StereoVision::~StereoVision() {
00029   //delete camera;
00030 }
00031 
00032 void StereoVision::evolve() {
00033   itsImgMutex.lock();
00034   if (itsFrameCount != lastFrameCount) {
00035     lastFrameCount = itsFrameCount;
00036     if (LFwdImage.initialized()) itsOfs->writeRGB(LFwdImage, "LeftFwdCamera");
00037     if (RFwdImage.initialized()) itsOfs2->writeRGB(RFwdImage, "RightFwdCamera");
00038     if (LDownImage.initialized())  itsOfs3->writeRGB(LDownImage, "LeftDownCamera");
00039     if (RDownImage.initialized()) itsOfs4->writeRGB(RDownImage, "RightDownCamera");
00040   }
00041   itsImgMutex.unlock();
00042 }
00043 
00044 void StereoVision::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00045                                  const Ice::Current&) {
00046 
00047   if (eMsg->ice_isA("::RobotSimEvents::RetinaMessage")) {
00048     RobotSimEvents::RetinaMessagePtr msg = RobotSimEvents::RetinaMessagePtr::dynamicCast(eMsg);
00049     itsImgMutex.lock();
00050     Image<PixRGB<byte> > image = Ice2Image<PixRGB<byte> >(msg->img);
00051     if (msg->cameraID == "LFwdCamera")        LFwdImage = image;
00052     else if (msg->cameraID == "RFwdCamera")   RFwdImage = image;
00053     else if (msg->cameraID == "LDownCamera")  LDownImage = image;
00054     else if (msg->cameraID == "RDownCamera")  RDownImage = image;
00055     itsFrameCount++;
00056     itsImgMutex.unlock();
00057   }
00058 }
00059 
00060 void StereoVision::registerTopics() {
00061   LINFO("Registering StereoVision Message");
00062   this->registerSubscription("RetinaMessageTopic"); 
00063 }
00064 
00065 #endif
00066