2 Copyright (C) 2012 Intel Corporation
4 This library is free software; you can redistribute it and/or
5 modify it under the terms of the GNU Lesser General Public
6 License as published by the Free Software Foundation; either
7 version 2.1 of the License, or (at your option) any later version.
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 Lesser General Public License for more details.
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19 #include "opencvluxplugin.h"
20 #include "timestamp.h"
21 #include <listplusplus.h>
22 #include <superptr.hpp>
23 #include <ambplugin.h>
27 #include <opencv2/opencv.hpp>
28 #include <opencv2/core/ocl.hpp>
31 #include <QFutureWatcher>
32 #include <QtConcurrent/QtConcurrent>
35 #include <opencv2/core/ocl.hpp>
39 const std::string VideoLogging = "VideoLogging";
40 const std::string DriverDrowsiness = "DriverDrowsiness";
41 const std::string OpenCL = "OpenCL";
45 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
47 auto plugin = new AmbPlugin<OpenCvLuxPlugin>(routingengine, config);
53 OpenCvLuxPlugin::OpenCvLuxPlugin(AbstractRoutingEngine* re, map<string, string> config, AbstractSource &parent)
54 :AmbPluginImpl(re, config, parent), lastLux(0), speed(0), latitude(0), longitude(0)
56 videoLogging = addPropertySupport(Zone::None, [](){
57 return new BasicPropertyType<bool>(VideoLogging, false);
60 driverDrowsiness = addPropertySupport(Zone::None, [](){
61 return new OpenCvLuxPlugin::DriverDrowsinessType(DriverDrowsiness, false);
64 openCl = addPropertySupport(Zone::None, [](){
65 return new BasicPropertyType<bool>(OpenCL, false);
68 extBrightness = addPropertySupport<VehicleProperty::ExteriorBrightnessType>(Zone::None);
70 shared = amb::make_unique(new Shared);
71 shared->parent = this;
73 shared->m_capture = NULL;
74 shared->mWriter = nullptr;
76 shared->threaded = false;
77 shared->kinect = false;
78 shared->useCuda = false;
79 shared->loggingOn = false;
84 shared->pixelLowerBound = 0;
85 shared->pixelUpperBound = 255;
88 if(config.find("threaded") != config.end())
90 shared->threaded = config["threaded"] == "true";
93 if(config.find("kinect") != config.end())
95 shared->kinect = config["kinect"] == "true";
98 if(config.find("fps") != config.end())
100 shared->fps = boost::lexical_cast<int>(config["fps"]);
103 if(config.find("device") != config.end())
105 device = config["device"].c_str();
108 if(config.find("pixelLowerBound") != config.end())
110 shared->pixelLowerBound = boost::lexical_cast<int>(config["pixelLowerBound"]);
112 if(shared->pixelLowerBound < 0)
114 shared->pixelLowerBound = 0;
118 if(config.find("pixelUpperBound") != config.end())
120 shared->pixelUpperBound = boost::lexical_cast<int>(config["pixelUpperBound"]);
122 if(shared->pixelUpperBound > 255)
123 shared->pixelUpperBound = 255;
126 if(config.find("opencl") != config.end())
128 if(cv::ocl::haveOpenCL())
130 bool useOcl = config["opencl"] == "true";
131 cv::ocl::setUseOpenCL(useOcl);
132 openCl->setValue(cv::ocl::useOpenCL());
136 DebugOut(DebugOut::Warning) << "You really don't have openCL support." << endl;
140 if(config.find("ddd") != config.end())
142 shared->ddd = config["ddd"] == "true";
145 if(config.find("logging") != config.end())
147 shared->loggingOn = config["logging"] == "true";
152 faceCascade = amb::make_unique(new cv::CascadeClassifier());
153 eyeCascade = amb::make_unique(new cv::CascadeClassifier());
155 std::string faceFile = config["faceCascade"];
157 if(!faceCascade->load(faceFile))
159 DebugOut(DebugOut::Warning) << "Could not load face cascade: " << faceFile <<". Disabling ddd" << endl;
163 std::string eyeFile = config["eyeCascade"];
165 if(!eyeCascade->load(eyeFile))
167 DebugOut(DebugOut::Warning) << "Could not load eye cascade: " << eyeFile <<". Disabling ddd" << endl;
173 routingEngine->subscribeToProperty(VehicleProperty::VehicleSpeed, &parent);
174 routingEngine->subscribeToProperty(VehicleProperty::Latitude, &parent);
175 routingEngine->subscribeToProperty(VehicleProperty::Longitude, &parent);
178 OpenCvLuxPlugin::~OpenCvLuxPlugin()
183 const string OpenCvLuxPlugin::uuid() const
185 return "3c7a1ea0-7d2e-11e2-9e96-0800200c9a66";
188 void OpenCvLuxPlugin::getPropertyAsync(AsyncPropertyReply *reply)
190 reply->timedout = [this](AsyncPropertyReply* reply) {
191 removeOne(&replyQueue, reply);
194 if(!shared->m_capture || !shared->m_capture->isOpened())
196 /// we want to turn on the camera for one shot to get an image and determine the intensity
199 grabImage(shared.get());
202 AmbPluginImpl::getPropertyAsync(reply);
205 void OpenCvLuxPlugin::getRangePropertyAsync(AsyncRangePropertyReply *reply)
207 throw std::runtime_error("OpenCVLuxPlugin does not support this operation. We should never hit this method.");
210 AsyncPropertyReply *OpenCvLuxPlugin::setProperty(AsyncSetPropertyRequest request )
212 if(request.property == VideoLogging)
214 shared->loggingOn = request.value->value<bool>();
216 if(request.property == OpenCL)
218 QMutexLocker lock(&mutex);
219 cv::ocl::setUseOpenCL(request.value->value<bool>());
222 return AmbPluginImpl::setProperty(request);;
225 void OpenCvLuxPlugin::subscribeToPropertyChanges(VehicleProperty::Property property)
227 if(!shared->mRequests.size())
230 g_timeout_add(1000 / shared->fps, grabImage, shared.get());
233 shared->mRequests.push_back(property);
236 void OpenCvLuxPlugin::unsubscribeToPropertyChanges(VehicleProperty::Property property)
238 removeOne(&shared->mRequests,property);
241 void OpenCvLuxPlugin::propertyChanged(AbstractPropertyType *value)
243 QMutexLocker lock(&mutex);
244 if(value->name == VehicleProperty::VehicleSpeed)
246 speed = value->value<uint16_t>();
248 else if(value->name == VehicleProperty::Latitude)
250 latitude = value->value<double>();
252 else if(value->name == VehicleProperty::Longitude)
254 longitude = value->value<double>();
258 void OpenCvLuxPlugin::supportedChanged(const PropertyList &)
260 DebugOut()<<"OpenCvLuxPlugin::supported changed."<<endl;
263 static int grabImage(void *data)
265 OpenCvLuxPlugin::Shared* shared = static_cast<OpenCvLuxPlugin::Shared*>(data);
267 shared->frameCount++;
269 if(shared->frameCount >= shared->fps)
270 shared->frameCount = 0;
272 if(shared->frameCount % 4 != 0)
277 if(!shared->m_capture->isOpened())
282 *(shared->m_capture.get()) >> m_image;
288 shared->parent->detectEyes(m_image);
292 DebugOut(DebugOut::Warning) << "DDD detection exception: "<< endl;
298 QFutureWatcher<uint> *watcher = new QFutureWatcher<uint>();
299 QObject::connect(watcher, &QFutureWatcher<uint>::finished, shared->parent, &OpenCvLuxPlugin::imgProcResult);
301 QFuture<uint> future = QtConcurrent::run(evalImage, m_image, shared);
302 watcher->setFuture(future);
304 QtConcurrent::run(shared->parent, &OpenCvLuxPlugin::writeVideoFrame, m_image);
308 if(shared->parent->videoLogging->value<bool>())
309 shared->parent->writeVideoFrame(m_image);
312 uint16_t lux = evalImage(m_image, shared);
313 shared->parent->updateProperty(lux);
317 DebugOut(DebugOut::Warning) << "Exception caught "<< __FUNCTION__ << endl;
320 //detectLight(m_image,shared);
323 if(shared->mRequests.size())
331 static uint evalImage(cv::UMat qImg, OpenCvLuxPlugin::Shared *shared)
335 DebugOut(DebugOut::Warning)<<"Empty image frame."<<endl;
339 cv::Scalar avgPixelIntensity;
341 avgPixelIntensity = cv::mean(qImg);
343 double val = avgPixelIntensity.val[0];
345 double qualifiedPixel = (val - shared->pixelLowerBound);
347 if(qualifiedPixel < 0) qualifiedPixel = 0;
349 uint lux = qualifiedPixel * (130000.0 / shared->pixelUpperBound);
351 DebugOut(7)<<"average pixel value: "<<qualifiedPixel<<" lux: "<<lux<<endl;
357 void OpenCvLuxPlugin::init()
359 if(!shared->m_capture)
362 shared->m_capture = amb::make_unique(new cv::VideoCapture(0));
364 shared->m_capture = amb::make_unique(new cv::VideoCapture(atoi(device.c_str())));
367 if(!shared->m_capture->isOpened())
369 DebugOut() << "we failed to open camera device (" << device << ") or no camera found" << endl;
373 if(!shared->mWriter || !shared->mWriter->isOpened())
375 cv::Size s = cv::Size((int) shared->m_capture->get(cv::CAP_PROP_FRAME_WIDTH),
376 (int) shared->m_capture->get(cv::CAP_PROP_FRAME_HEIGHT));
378 std::string codec = configuration["codec"];
380 if(codec.empty() || codec.size() != 4)
382 DebugOut(DebugOut::Warning)<<"Invalid codec: "<<codec <<". Using default: MJPG"<<endl;
386 std::string filename = configuration["logfile"];
388 if(filename.empty()) filename = "/tmp/video.avi";
390 boost::algorithm::to_upper(codec);
392 shared->mWriter = amb::make_unique(new cv::VideoWriter(filename, cv::VideoWriter::fourcc(codec.at(0), codec.at(1), codec.at(2), codec.at(3)),30,s));
395 DebugOut()<<"camera frame width: "<<shared->m_capture->get(cv::CAP_PROP_FRAME_WIDTH)<<endl;
396 DebugOut()<<"camera frame height: "<<shared->m_capture->get(cv::CAP_PROP_FRAME_HEIGHT)<<endl;
397 DebugOut()<<"camera frame fps: "<<shared->m_capture->get(cv::CAP_PROP_FPS)<<endl;
404 void OpenCvLuxPlugin::writeVideoFrame(cv::UMat f)
406 QMutexLocker locker(&mutex);
408 if(shared->loggingOn && speed > 0)
413 std::stringstream str;
414 str<<"Speed: "<<speed<<" kph, Lat/Lon: "<<latitude<<"/"<<longitude;
416 std::stringstream datestr;
417 datestr<<QDateTime::currentDateTime().toString().toStdString();
421 std::string text = str.str();
423 int fontFace = cv::FONT_HERSHEY_SIMPLEX;
424 double fontScale = 1;
428 cv::Size textSize = cv::getTextSize(text, fontFace,
429 fontScale, thickness, &baseline);
431 cv::Size dateTextSize = cv::getTextSize(datestr.str(), fontFace,
432 fontScale, thickness, &baseline);
434 baseline += thickness;
437 cv::Point textOrg((frame.cols - textSize.width)/2,
438 (frame.rows - textSize.height));
440 cv::Point dateOrg((frame.cols - dateTextSize.width)/2, dateTextSize.height);
442 // then put the text itself
443 cv::putText(frame, text, textOrg, fontFace, fontScale,
444 cv::Scalar::all(255), thickness, 8);
446 cv::putText(frame, datestr.str(), dateOrg, fontFace, fontScale,
447 cv::Scalar::all(255), thickness, 8);
449 (*shared->mWriter) << frame;
450 (*shared->mWriter) << frame;
454 void OpenCvLuxPlugin::updateProperty(uint16_t lux)
456 extBrightness->setValue(lux);
458 if(lux != lastLux && shared->mRequests.size())
461 routingEngine->updateProperty(extBrightness.get(), uuid());
465 void OpenCvLuxPlugin::imgProcResult()
469 QFutureWatcher<uint> *watcher = dynamic_cast<QFutureWatcher<uint>*>(sender());
471 uint lux = watcher->result();
472 shared->parent->updateProperty(lux);
474 watcher->deleteLater();
478 DebugOut(DebugOut::Warning) << "exception caught getting img processing result" << endl;
482 TrafficLight::Color detectLight(cv::UMat img, OpenCvLuxPlugin::Shared *shared)
487 cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
488 cv::GaussianBlur(gray, gray, cv::Size(9,9), 2, 2);
490 std::vector<cv::Vec3f> circles;
492 //cv::HoughCircles(gray, circles, CV_HOUGH_GRADIENT, 1, gray.rows/8, 200, 100, 0, 0);
494 cv::HoughCircles( gray, circles, cv::HOUGH_GRADIENT, 2, 30, 231, 50, 0, 0 );
496 for(auto i = 0; i < circles.size(); i++)
498 cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
499 int radius = cvRound(circles[i][2]);
501 cv::Rect rect(center.x - radius / 2, center.y - radius / 2, radius, radius);
505 cv::UMat light(img, rect);
507 cv::circle(img, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
509 cv::rectangle(img, rect.tl(), rect.br(), cv::Scalar(255,0,0));
511 cv::Scalar avgPixel = cv::mean(light);
513 if(avgPixel[2] > 128 && avgPixel[0] < 128 && avgPixel[1] < 128)
515 DebugOut(1)<<"Red Light!!!"<<endl;
516 return TrafficLight::Red;
518 else if(avgPixel[1] > 128 && avgPixel[0] < 128 && avgPixel[2] < 128)
520 DebugOut(1)<<"Green Light!!!"<<endl;
521 return TrafficLight::Green;
523 else if(avgPixel[0] > 128 && avgPixel[1] < 128 && avgPixel[2] < 128)
525 DebugOut(1)<<"Yellow Light!!!"<<endl;
526 return TrafficLight::Yellow;
538 void OpenCvLuxPlugin::detectEyes(cv::UMat frame)
543 bool hasEyes = false;
544 std::vector<cv::Rect> faces;
548 cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY);
549 cv::equalizeHist(frameGray, frameGray);
550 faceCascade->detectMultiScale(frameGray, faces, 1.1, 2, 0 | cv::CASCADE_SCALE_IMAGE, cv::Size(30, 30));
552 for( size_t i = 0; i < faces.size(); i++ )
554 cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
555 cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 255, 0, 255 ), 4, 8, 0 );
557 std::vector<cv::Rect> eyes;
560 cv::UMat faceROI(frameGray(faces[i]));
561 eyeCascade->detectMultiScale( faceROI, eyes, 1.1, 2, 0 | cv::CASCADE_SCALE_IMAGE, cv::Size(30, 30) );
563 for( size_t j = 0; j < eyes.size(); j++ )
565 cv::Point center( faces[i].x + eyes[j].x + eyes[j].width*0.5, faces[i].y + eyes[j].y + eyes[j].height*0.5 );
566 int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
567 cv::circle( frame, center, radius, cv::Scalar( 255, 0, 0 ), 4, 8, 0 );
573 DebugOut() << "Number of eyes: " << eyes.size() << endl;
575 if(driverDrowsiness->value<bool>())
577 driverDrowsiness->setValue(false);
578 routingEngine->updateProperty(driverDrowsiness.get(), this->uuid());
583 if(!driverDrowsiness->value<bool>())
585 driverDrowsiness->setValue(true);
586 routingEngine->updateProperty(driverDrowsiness.get(), this->uuid());