ColorSegmenterI.C

00001 #include "Robots/SeaBeeIII/ColorSegmenterI.H"
00002 
00003 #include "Component/ModelParam.H"
00004 #include "Component/ModelOptionDef.H"
00005 
00006 #include "Raster/Raster.H"
00007 
00008 #ifndef COLORSEGMENTERI_C
00009 #define COLORSEGMENTERI_C
00010 
00011 // ######################################################################
00012 ColorSegmenterI::ColorSegmenterI(OptionManager& mgr,
00013                 const std::string& descrName, const std::string& tagName) :
00014         VisionBrainComponentI(mgr, descrName, tagName), itsInit(false),
00015                         itsBuoySegmenter(2), itsPipeSegmenter(2)
00016 {
00017 
00018 }
00019 
00020 // ######################################################################
00021 ColorSegmenterI::~ColorSegmenterI()
00022 {
00023 }
00024 
00025 void ColorSegmenterI::registerTopics()
00026 {
00027         registerPublisher("RetinaMessageTopic");
00028         registerPublisher("BuoyColorSegmentMessageTopic");
00029         registerPublisher("PipeColorSegmentMessageTopic");
00030         //registerSubscription("BuoyColorSegmentConfigMessageTopic");
00031         //registerSubscription("PipeColorSegmentConfigMessageTopic");
00032         registerVisionTopics();
00033 }
00034 
00035 void ColorSegmenterI::updateFrame(Image<PixRGB<byte> > img, string cameraId)
00036 {
00037         bool isFwdCamera = false;
00038         if (cameraId == "FwdCamera") //once the L/RDnCamera identities work, we'll look for orange in Dn and red in Fwd
00039                 isFwdCamera = true;
00040 
00041         updateFrame(img, isFwdCamera);
00042 
00043 }
00044 
00045 void ColorSegmenterI::updateFrame(Image<PixRGB<byte> > img, bool isFwdCamera)
00046 {
00047         //LINFO("Image Recieved");
00048 
00049         if (img.initialized())
00050         {
00051                 /* Initializates itsColorSegmenter with the dimensions
00052                  of the images we are going to be receiving. Runs only
00053                  once.*/
00054                 if (!itsInit)
00055                 {
00056                         LINFO("here\n");
00057                         /*
00058                          SIsetXXX(double center value, double thresh = how much data can deviate from center,
00059                          double skew(+- indicates side of skew, magnitude is amount to extend bounds on that side))
00060                          */
00061                         //uncommented values are values that work well for footage
00062                         //itsBuoySegmenter.SIsetHue(320, 85, 0);
00063                         //itsBuoySegmenter.SIsetSat(55, 45, 0);
00064                         //itsBuoySegmenter.SIsetVal(128, 128, 0);
00065                         //^old values for red
00066 
00067                         //new values for red
00068                         itsBuoySegmenter.SIsetHue(352, 22, 0);
00069                         itsBuoySegmenter.SIsetSat(55, 42, 0);
00070                         itsBuoySegmenter.SIsetVal(128, 128, 0);
00071 
00072                         //itsPipeSegmenter.SIsetHue(150, 50, 0);
00073                         //itsPipeSegmenter.SIsetSat(28, 18, 0);
00074                         ////itsPipeSegmenter.SIsetSat(34,24,0);
00075                         //itsPipeSegmenter.SIsetVal(165.75, 38.25, 0);
00076                         //^old values for yellow
00077 
00078                         //new values for orange
00079                         itsPipeSegmenter.SIsetHue(22, 10, 0);
00080                         itsPipeSegmenter.SIsetSat(55, 30, 0);
00081                         itsPipeSegmenter.SIsetVal(128, 128, 0);
00082 
00083                         //SIsetFrame(x1,y1,x2,y2,realX,realY) limits the consideration into the rectangle from (x1,y1) to (x2,y2)
00084                         itsBuoySegmenter.SIsetFrame(0, 0, img.getWidth(), img.getHeight(),
00085                                         img.getWidth(), img.getHeight());
00086                         itsPipeSegmenter.SIsetFrame(0, 0, img.getWidth(), img.getHeight(),
00087                                         img.getWidth(), img.getHeight());
00088                         itsInit = true;
00089                 }
00090 
00091                 Image<PixRGB<float> > display(img);
00092 
00093                 //segmenters do the color segmenting here
00094                 itsBuoySegmenter.SIsegment(display);
00095                 itsPipeSegmenter.SIsegment(display);
00096 
00097                 //Candidates are pixels that fit within the accepted range that also fit in the previous frame (noise elimination)
00098                 Image<bool> buoyCand = itsBuoySegmenter.SIreturnCandidates();
00099                 Image<bool> pipeCand = itsPipeSegmenter.SIreturnCandidates();
00100 
00101                 ///*The code between this ///* and the following //*/ can be commented out when not looking at output to improve performance.
00102                 //work-around to get writeRGB to display something visible;
00103                 Image<PixRGB<byte> > segImgDisp; //Image to display to screen
00104                 segImgDisp.resize(buoyCand.getWidth(), buoyCand.getHeight());
00105 
00106                 for (int i = 0; i < buoyCand.getWidth(); ++i)
00107                 {
00108                         for (int j = 0; j < buoyCand.getHeight(); ++j)
00109                         {
00110                                 //LINFO("x=%d,y=%d",i,j);
00111                                 // if(buoyCand.getVal(i,j) and pipeCand.getVal(i,j)){
00112                                 //   segImgDisp.setVal(i,j,PixRGB<byte>(255,128,0));
00113                                 //}
00114                                 if (buoyCand.getVal(i, j))
00115                                 {
00116                                         segImgDisp.setVal(i, j, PixRGB<byte> (255, 0, 0));
00117                                 }
00118                                 else if (pipeCand.getVal(i, j))
00119                                 {
00120                                         segImgDisp.setVal(i, j, PixRGB<byte> (255, 127, 0));
00121                                 }
00122                                 else
00123                                 {
00124                                         segImgDisp.setVal(i, j, PixRGB<byte> (0, 0, 255));
00125                                 }
00126                         }
00127                 }
00128 
00129                 itsOfs->writeRGB(concatLooseX(img, segImgDisp, PixRGB<byte> (0, 0, 0)),
00130                                 "Color Segmenter Image", FrameInfo("Color Segementer", SRC_POS));
00131 
00132                 //*/Code block can be commented out when not debugging output
00133 
00134                 //sends out Retina Messages with the candidate images
00135                 RobotSimEvents::RetinaMessagePtr msg =
00136                                 new RobotSimEvents::RetinaMessage;
00137                 msg->img = Image2Ice(buoyCand);
00138                 msg->cameraID = "BuoyColorSegmenter";
00139 
00140                 this->publish("RetinaMessageTopic", msg);
00141 
00142                 msg = new RobotSimEvents::RetinaMessage;
00143                 msg->img = Image2Ice(pipeCand);
00144                 msg->cameraID = "PipeColorSegmenter";
00145 
00146                 this->publish("RetinaMessageTopic", msg);
00147 
00148                 //blob properties calculated here
00149                 itsBuoySegmenter.SIcalcMassCenter();
00150                 itsPipeSegmenter.SIcalcMassCenter();
00151 
00152                 //the following code puts the info about the red blobs (buoy) and yellow blobs(pipe) into a sorted array to send out as location messages
00153                 int i = 0;
00154                 vector<pair<float, pair<int, int> > > redBlobs(
00155                                 itsBuoySegmenter.SInumberBlobs());
00156                 for (i = 0; i < itsBuoySegmenter.SInumberBlobs(); ++i)
00157                 {
00158                         redBlobs[i] = pair<float, pair<int, int> > (
00159                                         itsBuoySegmenter.SIgetMass(i), pair<int, int> (
00160                                                         itsBuoySegmenter.SIgetCenterX(i),
00161                                                         itsBuoySegmenter.SIgetCenterY(i)));
00162                         /*LINFO("Red Segmenter Blob %d: center location: (%f, %f) mass: %ld, Xrange: [%d, %d], Yrange: [%d, %d]\n",
00163                                         i, itsBuoySegmenter.SIgetCenterX(i),itsBuoySegmenter.SIgetCenterY(i),itsBuoySegmenter.SIgetMass(i),
00164                                         itsBuoySegmenter.SIgetXmin(i),itsBuoySegmenter.SIgetXmax(i),itsBuoySegmenter.SIgetYmin(i),itsBuoySegmenter.SIgetYmax(i));
00165                                         */
00166                 }
00167                 vector<pair<float, pair<int, int> > > yellowBlobs(
00168                                 itsPipeSegmenter.SInumberBlobs());
00169                 for (i = 0; i < itsPipeSegmenter.SInumberBlobs(); ++i)
00170                 {
00171                         yellowBlobs[i] = pair<float, pair<int, int> > (
00172                                         itsPipeSegmenter.SIgetMass(i), pair<int, int> (
00173                                                         itsPipeSegmenter.SIgetCenterX(i),
00174                                                         itsPipeSegmenter.SIgetCenterY(i)));
00175                         /*LINFO("Yellow Segmenter Blob %d: center location: (%f, %f) mass: %ld, Xrange: [%d, %d], Yrange: [%d, %d]\n",
00176                                         i, itsPipeSegmenter.SIgetCenterX(i),itsPipeSegmenter.SIgetCenterY(i),itsPipeSegmenter.SIgetMass(i),
00177                                         itsPipeSegmenter.SIgetXmin(i),itsPipeSegmenter.SIgetXmax(i),itsPipeSegmenter.SIgetYmin(i),itsPipeSegmenter.SIgetYmax(i));*/
00178                 }
00179                 sort(redBlobs.begin(), redBlobs.end());
00180                 sort(yellowBlobs.begin(), yellowBlobs.end());
00181 
00182                 //for each blob, sends out a message with center location and size
00183                 RobotSimEvents::BuoyColorSegmentMessagePtr msg2;
00184                 for (i = itsBuoySegmenter.SInumberBlobs() - 1; i >= 0; --i)
00185                 {
00186                         msg2 = new RobotSimEvents::BuoyColorSegmentMessage;
00187                         msg2->x = float(redBlobs[i].second.first) / float(img.getWidth());
00188                         msg2->y = float(redBlobs[i].second.second) / float(img.getHeight());
00189                         msg2->size = redBlobs[i].first;
00190 
00191                         this->publish("BuoyColorSegmentMessageTopic", msg2);
00192                 }
00193                 RobotSimEvents::PipeColorSegmentMessagePtr msg3;
00194                 //cout << "yellow blobs:" << endl;
00195                 for (i = itsPipeSegmenter.SInumberBlobs() - 1; i >= 0; --i)
00196                 {
00197                         msg3 = new RobotSimEvents::PipeColorSegmentMessage;
00198                         msg3->x = float(yellowBlobs[i].second.first)
00199                                         / float(img.getWidth());
00200                         msg3->y = float(yellowBlobs[i].second.second)
00201                                         / float(img.getHeight());
00202                         msg3->size = yellowBlobs[i].first;
00203                         /*if(msg3->size > 1000)
00204                         {
00205                                 cout << ">" << msg3->size << endl;
00206                         }*/
00207 
00208                         this->publish("PipeColorSegmentMessageTopic", msg3);
00209                 }
00210 
00211         }
00212 }
00213 /*    void ColorSegmenterI::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00214  const Ice::Current&)
00215  {
00216  //if message is a Buoy config message, sets buoy segmenter settings
00217  if(eMsg->ice_isA("::RobotSimEvents::BuoyColorSegmentConfigMessage")){
00218  RobotSimEvents::BuoyColorSegmentConfigMessagePtr msg = RobotSimEvents::BuoyColorSegmentConfigMessagePtr::dynamicCast(eMsg);
00219  printf("Got A Buoy ColorSegment Config Message! HUT: %f HLT: %f SUT: %f SLT: %f VUT: %f VLT: %f", msg->HUT, msg->HLT, msg->SUT, msg->SLT, msg->VUT, msg->VLT);
00220  //SIsetXXX(center,deviation,skew), so this calculates center using avg of bounds and deviation as twice the diff of bounds
00221  itsBuoySegmenter.SIsetHue((msg->HUT+msg->HLT)/2,(msg->HUT-msg->HLT)/2,0);
00222  itsBuoySegmenter.SIsetSat((msg->SUT+msg->SLT)/2,(msg->SUT-msg->SLT)/2,0);
00223  itsBuoySegmenter.SIsetVal((msg->VUT+msg->VLT)/2,(msg->VUT-msg->VLT)/2,0);
00224  }
00225  //if message is a Pipe config message, sets pipe segmenter settings
00226  else if(eMsg->ice_isA("::RobotSimEvents::PipeColorSegmentConfigMessage")){
00227  RobotSimEvents::PipeColorSegmentConfigMessagePtr msg = RobotSimEvents::PipeColorSegmentConfigMessagePtr::dynamicCast(eMsg);
00228  printf("Got A Pipe ColorSegment Config Message! HUT: %f HLT: %f SUT: %f SLT: %f VUT: %f VLT: %f", msg->HUT, msg->HLT, msg->SUT, msg->SLT, msg->VUT, msg->VLT);
00229  itsPipeSegmenter.SIsetHue((msg->HUT+msg->HLT)/2,(msg->HUT-msg->HLT)/2,0);
00230  itsPipeSegmenter.SIsetSat((msg->SUT+msg->SLT)/2,(msg->SUT-msg->SLT)/2,0);
00231  itsPipeSegmenter.SIsetVal((msg->VUT+msg->VLT)/2,(msg->VUT-msg->VLT)/2,0);
00232  }
00233  }*/
00234 
00235 #endif