diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt index 273255c5..7f73f477 100644 --- a/app/CMakeLists.txt +++ b/app/CMakeLists.txt @@ -5,7 +5,9 @@ SET(headers_ui ../src/MainWindow.h ../src/AddObjectDialog.h ../src/ObjWidget.h + ../src/FindObject.h ../src/Camera.h + ../src/CameraTcpClient.h ../src/ParametersToolBox.h ../src/AboutDialog.h ../src/TcpServer.h @@ -43,12 +45,15 @@ SET(SRC_FILES ../src/RectItem.cpp ../src/QtOpenCV.cpp ../src/Camera.cpp + ../src/CameraTcpClient.cpp ../src/ParametersToolBox.cpp ../src/Settings.cpp ../src/ObjWidget.cpp + ../src/FindObject.cpp ../src/AboutDialog.cpp ../src/TcpServer.cpp ../src/Vocabulary.cpp + ../src/utilite/ULogger.cpp ../src/utilite/UPlot.cpp ../src/utilite/UDirectory.cpp ../src/utilite/UFile.cpp diff --git a/app/main.cpp b/app/main.cpp index 30f8b124..72f8b14f 100644 --- a/app/main.cpp +++ b/app/main.cpp @@ -3,6 +3,12 @@ #include #include "MainWindow.h" #include "Settings.h" +#include "FindObject.h" +#include "Camera.h" +#include "TcpServer.h" +#include "utilite/ULogger.h" + +bool running = true; #ifdef WIN32 #include @@ -11,19 +17,47 @@ BOOL WINAPI my_handler(DWORD signal) if (signal == CTRL_C_EVENT) { printf("\nCtrl-C caught! Quitting application...\n"); - QApplication::quit(); + QCoreApplication::quit(); } return TRUE; + running = false; } #else #include void my_handler(int s) { printf("\nCtrl-C caught! Quitting application...\n"); - QApplication::quit(); + QCoreApplication::quit(); + running = false; +} +inline void Sleep(unsigned int ms) +{ + struct timespec req; + struct timespec rem; + req.tv_sec = ms / 1000; + req.tv_nsec = (ms - req.tv_sec * 1000) * 1000 * 1000; + nanosleep (&req, &rem); } #endif +void setupQuitSignal() +{ +// Catch ctrl-c to close Qt +#ifdef WIN32 + if (!SetConsoleCtrlHandler(my_handler, TRUE)) + { + printf("\nERROR: Could not set control handler"); + return 1; + } +#else + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = my_handler; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); +#endif +} + void showUsage() { printf("\nUsage:\n" @@ -44,9 +78,14 @@ void showUsage() int main(int argc, char* argv[]) { - QApplication app(argc, argv); + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kInfo); + ULogger::setPrintWhere(false); + ULogger::setPrintTime(false); - // parse options + ////////////////////////// + // parse options BEGIN + ////////////////////////// bool guiMode = true; QString objectsPath = ""; QString configPath = Settings::iniDefaultPath(); @@ -65,7 +104,7 @@ int main(int argc, char* argv[]) } if(!QDir(objectsPath).exists()) { - printf("[ERROR] Path not valid : %s\n", objectsPath.toStdString().c_str()); + UERROR("Path not valid : %s", objectsPath.toStdString().c_str()); showUsage(); } } @@ -87,7 +126,7 @@ int main(int argc, char* argv[]) } if(!QFile::exists(configPath)) { - printf("[ERROR] Configuration file doesn't exist : %s\n", configPath.toStdString().c_str()); + UERROR("Configuration file doesn't exist : %s", configPath.toStdString().c_str()); showUsage(); } } @@ -107,61 +146,93 @@ int main(int argc, char* argv[]) showUsage(); } - printf("[ERROR] Unrecognized option : %s\n", argv[i]); + UERROR("Unrecognized option : %s", argv[i]); showUsage(); } - printf("Options:\n"); - printf(" GUI mode = %s\n", guiMode?"true":"false"); - printf(" Objects path: \"%s\"\n", objectsPath.toStdString().c_str()); - printf(" Settings path: \"%s\"\n", configPath.toStdString().c_str()); + UINFO("Options:"); + UINFO(" GUI mode = %s", guiMode?"true":"false"); + UINFO(" Objects path: \"%s\"", objectsPath.toStdString().c_str()); + UINFO(" Settings path: \"%s\"", configPath.toStdString().c_str()); + ////////////////////////// + // parse options END + ////////////////////////// - MainWindow mainWindow(0, configPath); + // Load settings, should be loaded before creating other objects + Settings::init(configPath); + // Create FindObject + FindObject * findObject = new FindObject(); + + // Load objects if path is set int objectsLoaded = 0; if(!objectsPath.isEmpty()) { - objectsLoaded = mainWindow.loadObjects(objectsPath); + objectsLoaded = findObject->loadObjects(objectsPath); if(!objectsLoaded) { - printf("[WARNING] No objects loaded from \"%s\"\n", objectsPath.toStdString().c_str()); + UWARN("No objects loaded from \"%s\"", objectsPath.toStdString().c_str()); } } - if(objectsLoaded == 0 && !guiMode) - { - printf("[ERROR] In console mode, at least one object must be loaded! See -console option.\n"); - showUsage(); - } - if(guiMode) { + QApplication app(argc, argv); + MainWindow mainWindow(findObject, 0); // ownership transfered + app.connect( &app, SIGNAL( lastWindowClosed() ), &app, SLOT( quit() ) ); mainWindow.show(); + + app.exec(); } else { - mainWindow.startProcessing(); - } - - if(!guiMode) - { - // Catch ctrl-c to close the gui -#ifdef WIN32 - if (!SetConsoleCtrlHandler(my_handler, TRUE)) + if(objectsLoaded == 0) { - printf("\nERROR: Could not set control handler"); - return 1; + UERROR("In console mode, at least one object must be loaded! See -console option."); + delete findObject; + showUsage(); } -#else - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = my_handler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); -#endif + + QCoreApplication app(argc, argv); + Camera camera; + TcpServer tcpServer(Settings::getGeneral_port()); + printf("IP: %s\nport: %d\n", tcpServer.getHostAddress().toString().toStdString().c_str(), tcpServer.getPort()); + + // connect stuff: + // [Camera] ---Image---> [FindObject] ---ObjectsDetected---> [TcpServer] + QObject::connect(&camera, SIGNAL(imageReceived(const cv::Mat &)), findObject, SLOT(detect(const cv::Mat &))); + QObject::connect(findObject, SIGNAL(objectsFound(QMultiMap >)), &tcpServer, SLOT(publishObjects(QMultiMap >))); + + setupQuitSignal(); + + // start processing! + while(running && !camera.start()) + { + if(Settings::getCamera_6useTcpCamera()) + { + UWARN("Camera initialization failed! (with server %s:%d) Trying again in 1 second...", + Settings::getCamera_7IP().toStdString().c_str(), Settings::getCamera_8port()); + Sleep(1000); + } + else + { + UERROR("Camera initialization failed!"); + running = false; + } + } + if(running) + { + app.exec(); + } + + // cleanup + camera.stop(); + delete findObject; + tcpServer.close(); } - return app.exec(); + // Save settings + Settings::saveSettings(); } diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 82ab8676..c38f8eb0 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -2,6 +2,7 @@ SET(headers_ui ../src/ObjWidget.h ../src/Camera.h + ../src/CameraTcpClient.h ) #This will generate moc_* for Qt QT4_WRAP_CPP(moc_srcs ${headers_ui}) @@ -14,8 +15,11 @@ SET(SRC_FILES ../src/QtOpenCV.cpp ../src/Settings.cpp ../src/Camera.cpp + ../src/CameraTcpClient.cpp ../src/utilite/UDirectory.cpp ../src/utilite/UFile.cpp + ../src/utilite/ULogger.cpp + ../src/utilite/UConversion.cpp ${moc_srcs} ) diff --git a/example/main.cpp b/example/main.cpp index 44272a69..ce33f1c4 100644 --- a/example/main.cpp +++ b/example/main.cpp @@ -151,8 +151,8 @@ int main(int argc, char * argv[]) // PROCESS NEAREST NEIGHBOR RESULTS //////////////////////////// // Set gui data - objWidget.setData(objectKeypoints, objectDescriptors, objectImg, "", ""); - sceneWidget.setData(sceneKeypoints, sceneDescriptors, sceneImg, "", ""); + objWidget.setData(objectKeypoints, cvtCvMat2QImage(objectImg)); + sceneWidget.setData(sceneKeypoints, cvtCvMat2QImage(sceneImg)); // Find correspondences by NNDR (Nearest Neighbor Distance Ratio) float nndrRatio = 0.8; diff --git a/imagesTcpServer/CMakeLists.txt b/imagesTcpServer/CMakeLists.txt index 3f8d6e70..16ddc522 100644 --- a/imagesTcpServer/CMakeLists.txt +++ b/imagesTcpServer/CMakeLists.txt @@ -1,6 +1,7 @@ ### Qt Gui stuff ### SET(headers_ui ../src/Camera.h + ../src/CameraTcpClient.h ImagesTcpServer.h ) #This will generate moc_* for Qt @@ -9,10 +10,13 @@ QT4_WRAP_CPP(moc_srcs ${headers_ui}) SET(SRC_FILES ../src/Camera.cpp + ../src/CameraTcpClient.cpp ../src/Settings.cpp ../src/QtOpenCV.cpp ../src/utilite/UDirectory.cpp ../src/utilite/UFile.cpp + ../src/utilite/ULogger.cpp + ../src/utilite/UConversion.cpp ImagesTcpServer.cpp main.cpp ${moc_srcs} diff --git a/src/AddObjectDialog.cpp b/src/AddObjectDialog.cpp index c6c146a8..c86388f7 100644 --- a/src/AddObjectDialog.cpp +++ b/src/AddObjectDialog.cpp @@ -9,6 +9,8 @@ #include "Camera.h" #include "QtOpenCV.h" #include "Settings.h" +#include "ObjSignature.h" +#include "utilite/ULogger.h" #include @@ -22,7 +24,8 @@ AddObjectDialog::AddObjectDialog(Camera * camera, const cv::Mat & image, bool mirrorView, QWidget * parent, Qt::WindowFlags f) : QDialog(parent, f), camera_(camera), - object_(0) + objWidget_(0), + objSignature_(0) { ui_ = new Ui_addObjectDialog(); ui_->setupUi(this); @@ -38,7 +41,7 @@ AddObjectDialog::AddObjectDialog(Camera * camera, const cv::Mat & image, bool mi connect(ui_->comboBox_selection, SIGNAL(currentIndexChanged(int)), this, SLOT(changeSelectionMode())); connect(ui_->cameraView, SIGNAL(selectionChanged()), this, SLOT(updateNextButton())); - connect(ui_->cameraView, SIGNAL(roiChanged(const QRect &)), this, SLOT(updateNextButton(const QRect &))); + connect(ui_->cameraView, SIGNAL(roiChanged(const cv::Rect &)), this, SLOT(updateNextButton(const cv::Rect &))); ui_->cameraView->setMirrorView(mirrorView); if((camera_ && camera_->isRunning()) || image.empty()) @@ -56,13 +59,27 @@ AddObjectDialog::~AddObjectDialog() { delete detector_; delete extractor_; - if(object_) + if(objWidget_) { - delete object_; + delete objWidget_; + objWidget_ = 0; + } + if(objSignature_) + { + delete objSignature_; + objSignature_ = 0; } delete ui_; } +void AddObjectDialog::retrieveObject(ObjWidget ** widget, ObjSignature ** signature) +{ + *widget = objWidget_; + objWidget_= 0; + *signature = objSignature_; + objSignature_ = 0; +} + void AddObjectDialog::closeEvent(QCloseEvent* event) { if(camera_) @@ -91,40 +108,40 @@ void AddObjectDialog::takePicture() void AddObjectDialog::updateNextButton() { - updateNextButton(QRect()); + updateNextButton(cv::Rect()); } -void AddObjectDialog::updateNextButton(const QRect & rect) +void AddObjectDialog::updateNextButton(const cv::Rect & rect) { roi_ = rect; - if(roi_.isValid() && ui_->cameraView->cvImage().cols) + if(roi_.height && roi_.width && cameraImage_.cols) { //clip roi - if( roi_.x() >= ui_->cameraView->cvImage().cols || - roi_.x()+roi_.width() <= 0 || - roi_.y() >= ui_->cameraView->cvImage().rows || - roi_.y()+roi_.height() <= 0) + if( roi_.x >= cameraImage_.cols || + roi_.x+roi_.width <= 0 || + roi_.y >= cameraImage_.rows || + roi_.y+roi_.height <= 0) { //Not valid... - roi_ = QRect(); + roi_ = cv::Rect(); } else { - if(roi_.x() < 0) + if(roi_.x < 0) { - roi_.setX(0); + roi_.x = 0; } - if(roi_.x() + roi_.width() > ui_->cameraView->cvImage().cols) + if(roi_.x + roi_.width > cameraImage_.cols) { - roi_.setWidth(ui_->cameraView->cvImage().cols - roi_.x()); + roi_.width = cameraImage_.cols - roi_.x; } - if(roi_.y() < 0) + if(roi_.y < 0) { - roi_.setY(0); + roi_.y = 0; } - if(roi_.y() + roi_.height() > ui_->cameraView->cvImage().rows) + if(roi_.y + roi_.height > cameraImage_.rows) { - roi_.setHeight(ui_->cameraView->cvImage().rows - roi_.y()); + roi_.height = cameraImage_.rows - roi_.y; } } } @@ -143,7 +160,7 @@ void AddObjectDialog::updateNextButton(const QRect & rect) } else { - if(roi_.isNull()) + if(roi_.width == 0 || roi_.height == 0) { ui_->pushButton_next->setEnabled(false); } @@ -214,7 +231,7 @@ void AddObjectDialog::setState(int state) ui_->label_instruction->setText(tr("Select region representing the object.")); ui_->cameraView->setGraphicsViewMode(false); } - updateNextButton(QRect()); + updateNextButton(cv::Rect()); } else if(state == kVerifySelection) { @@ -245,32 +262,25 @@ void AddObjectDialog::setState(int state) std::vector selectedKeypoints = ui_->cameraView->selectedKeypoints(); // Select keypoints - if(!cvImage_.empty() && + if(!cameraImage_.empty() && ((ui_->comboBox_selection->currentIndex() == 1 && selectedKeypoints.size()) || - (ui_->comboBox_selection->currentIndex() == 0 && !roi_.isNull()))) + (ui_->comboBox_selection->currentIndex() == 0 && roi_.width && roi_.height))) { - cv::Rect roi; if(ui_->comboBox_selection->currentIndex() == 1) { - roi = computeROI(selectedKeypoints); + roi_ = computeROI(selectedKeypoints); } - else - { - roi.x = roi_.x(); - roi.y = roi_.y(); - roi.width = roi_.width(); - roi.height = roi_.height(); - } - cv::Mat imgRoi(cvImage_, roi); + + cv::Mat imgRoi(cameraImage_, roi_); if(ui_->comboBox_selection->currentIndex() == 1) { - if(roi.x != 0 || roi.y != 0) + if(roi_.x != 0 || roi_.y != 0) { for(unsigned int i=0; idetect(imgRoi, selectedKeypoints); } - ui_->objectView->setData(selectedKeypoints, cv::Mat(), imgRoi, Settings::currentDetectorType(), ""); - ui_->objectView->setMinimumSize(roi.width, roi.height); + ui_->objectView->setData(selectedKeypoints, cvtCvMat2QImage(imgRoi.clone())); + ui_->objectView->setMinimumSize(roi_.width, roi_.height); ui_->objectView->update(); ui_->pushButton_next->setEnabled(true); } else { - printf("Please select items\n"); + UINFO("Please select items"); ui_->pushButton_next->setEnabled(false); } ui_->label_instruction->setText(tr("Selection : %1 features").arg(selectedKeypoints.size())); @@ -296,26 +306,34 @@ void AddObjectDialog::setState(int state) { std::vector keypoints = ui_->objectView->keypoints(); if((ui_->comboBox_selection->currentIndex() == 1 && keypoints.size()) || - (ui_->comboBox_selection->currentIndex() == 0 && !roi_.isNull())) + (ui_->comboBox_selection->currentIndex() == 0 && roi_.width && roi_.height)) { cv::Mat descriptors; + cv::Mat imgRoi(cameraImage_, roi_); if(keypoints.size()) { // Extract descriptors - extractor_->compute(ui_->objectView->cvImage(), keypoints, descriptors); + extractor_->compute(imgRoi, keypoints, descriptors); if(keypoints.size() != (unsigned int)descriptors.rows) { - printf("ERROR : keypoints=%d != descriptors=%d\n", (int)keypoints.size(), descriptors.rows); + UERROR("keypoints=%d != descriptors=%d", (int)keypoints.size(), descriptors.rows); } } - if(object_) + if(objWidget_) { - delete object_; - object_ = 0; + delete objWidget_; + objWidget_ = 0; } - object_ = new ObjWidget(0, keypoints, descriptors, ui_->objectView->cvImage(), Settings::currentDetectorType(), Settings::currentDescriptorType()); + if(objSignature_) + { + delete objSignature_; + objSignature_ = 0; + } + objSignature_ = new ObjSignature(0, imgRoi.clone()); + objSignature_->setData(keypoints, descriptors, Settings::currentDetectorType(), Settings::currentDescriptorType()); + objWidget_ = new ObjWidget(0, keypoints, cvtCvMat2QImage(imgRoi.clone())); this->accept(); } @@ -324,24 +342,24 @@ void AddObjectDialog::setState(int state) void AddObjectDialog::update(const cv::Mat & image) { - cvImage_ = cv::Mat(); + cameraImage_ = cv::Mat(); if(!image.empty()) { // convert to grayscale if(image.channels() != 1 || image.depth() != CV_8U) { - cv::cvtColor(image, cvImage_, CV_BGR2GRAY); + cv::cvtColor(image, cameraImage_, CV_BGR2GRAY); } else { - cvImage_ = image; + cameraImage_ = image.clone(); } // Extract keypoints cv::vector keypoints; - detector_->detect(cvImage_, keypoints); + detector_->detect(cameraImage_, keypoints); - ui_->cameraView->setData(keypoints, cv::Mat(), cvImage_, Settings::currentDetectorType(), ""); + ui_->cameraView->setData(keypoints, cvtCvMat2QImage(cameraImage_)); ui_->cameraView->update(); } } @@ -383,8 +401,8 @@ cv::Rect AddObjectDialog::computeROI(const std::vector & kpts) roi.y = h1; roi.width = x2-x1; roi.height = h2-h1; - //printf("ptx=%d, pty=%d\n", (int)kpts.at(i).pt.x, (int)kpts.at(i).pt.y); - //printf("x=%d, y=%d, w=%d, h=%d\n", roi.x, roi.y, roi.width, roi.height); + //UINFO("ptx=%d, pty=%d", (int)kpts.at(i).pt.x, (int)kpts.at(i).pt.y); + //UINFO("x=%d, y=%d, w=%d, h=%d", roi.x, roi.y, roi.width, roi.height); } return roi; diff --git a/src/AddObjectDialog.h b/src/AddObjectDialog.h index 28a019ec..7424b0a4 100644 --- a/src/AddObjectDialog.h +++ b/src/AddObjectDialog.h @@ -16,6 +16,7 @@ class Camera; class KeypointItem; class KeypointDetector; class DescriptorExtractor; +class ObjSignature; class AddObjectDialog : public QDialog { @@ -26,7 +27,7 @@ public: virtual ~AddObjectDialog(); // ownership transferred to caller - ObjWidget * retrieveObject() {ObjWidget * obj = object_; object_=0; return obj;} + void retrieveObject(ObjWidget ** widget, ObjSignature ** signature); private Q_SLOTS: void update(const cv::Mat &); @@ -35,7 +36,7 @@ private Q_SLOTS: void cancel(); void takePicture(); void updateNextButton(); - void updateNextButton(const QRect &); + void updateNextButton(const cv::Rect &); void changeSelectionMode(); protected: @@ -47,14 +48,15 @@ private: private: Ui_addObjectDialog * ui_; Camera * camera_; - ObjWidget * object_; - cv::Mat cvImage_; + ObjWidget * objWidget_; + ObjSignature * objSignature_; + cv::Mat cameraImage_; + cv::Rect roi_; KeypointDetector * detector_; DescriptorExtractor * extractor_; enum State{kTakePicture, kSelectFeatures, kVerifySelection, kClosing}; int state_; - QRect roi_; }; #endif /* ADDOBJECTDIALOG_H_ */ diff --git a/src/Camera.cpp b/src/Camera.cpp index 1951506d..9087e77d 100644 --- a/src/Camera.cpp +++ b/src/Camera.cpp @@ -8,93 +8,9 @@ #include "Settings.h" #include #include "utilite/UDirectory.h" +#include "utilite/ULogger.h" #include "QtOpenCV.h" -CameraTcpClient::CameraTcpClient(QObject *parent) : - QTcpSocket(parent), - blockSize_(0) -{ - connect(this, SIGNAL(readyRead()), this, SLOT(readReceivedData())); - connect(this, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(displayError(QAbstractSocket::SocketError))); - connect(this, SIGNAL(disconnected()), this, SLOT(connectionLost())); -} - -cv::Mat CameraTcpClient::getImage() -{ - cv::Mat img; - if(images_.size()) - { - // if queue changed after tcp connection ended with images still in the buffer - int queue = Settings::getCamera_9queueSize(); - while(queue > 0 && images_.size() > queue) - { - images_.pop_front(); - } - - img = images_.front(); - images_.pop_front(); - } - return img; -} - -void CameraTcpClient::readReceivedData() -{ - QDataStream in(this); - in.setVersion(QDataStream::Qt_4_0); - - if (blockSize_ == 0) - { - if (this->bytesAvailable() < (int)sizeof(quint64)) - { - return; - } - - in >> blockSize_; - } - - if (this->bytesAvailable() < (int)blockSize_) - { - return; - } - - std::vector buf(blockSize_); - in.readRawData((char*)buf.data(), blockSize_); - images_.push_back(cv::imdecode(buf, cv::IMREAD_UNCHANGED)); - int queue = Settings::getCamera_9queueSize(); - while(queue > 0 && images_.size() > queue) - { - images_.pop_front(); - } - blockSize_ = 0; -} - -void CameraTcpClient::displayError(QAbstractSocket::SocketError socketError) -{ - switch (socketError) - { - case QAbstractSocket::RemoteHostClosedError: - break; - case QAbstractSocket::HostNotFoundError: - printf("[ERROR] CameraTcp: Tcp error: The host was not found. Please " - "check the host name and port settings.\n"); - break; - case QAbstractSocket::ConnectionRefusedError: - printf("[ERROR] CameraTcp: The connection was refused by the peer. " - "Make sure your images server is running, " - "and check that the host name and port " - "settings are correct.\n"); - break; - default: - printf("The following error occurred: %s.\n", this->errorString().toStdString().c_str()); - break; - } -} - -void CameraTcpClient::connectionLost() -{ - //printf("[WARNING] CameraTcp: Connection lost!\n"); -} - Camera::Camera(QObject * parent) : QObject(parent), currentImageIndex_(0) @@ -179,7 +95,7 @@ void Camera::takeImage() img = cameraTcpClient_.getImage(); if(cameraTcpClient_.imagesBuffered() > 0 && Settings::getCamera_9queueSize() == 0) { - printf("[Warning] %d images buffered so far...\n", cameraTcpClient_.imagesBuffered()); + UWARN("%d images buffered so far...", cameraTcpClient_.imagesBuffered()); } while(img.empty() && cameraTcpClient_.waitForReadyRead()) { @@ -189,7 +105,7 @@ void Camera::takeImage() { if(!cameraTcpClient_.waitForConnected()) { - printf("Connection is lost, trying to reconnect to server (%s:%d)... (at the rate of the camera: %d ms)\n", + UWARN("Connection is lost, trying to reconnect to server (%s:%d)... (at the rate of the camera: %d ms)", Settings::getCamera_7IP().toStdString().c_str(), Settings::getCamera_8port(), cameraTimer_.interval()); @@ -200,7 +116,7 @@ void Camera::takeImage() if(img.empty()) { - printf("Camera: Could not grab a frame, the end of the feed may be reached...\n"); + UWARN("Camera: Could not grab a frame, the end of the feed may be reached..."); } else { @@ -234,7 +150,7 @@ bool Camera::start() cameraTcpClient_.connectToHost(Settings::getCamera_7IP(), Settings::getCamera_8port()); if(!cameraTcpClient_.waitForConnected()) { - printf("[WARNING] Camera: Cannot connect to server \"%s:%d\"\n", + UWARN("Camera: Cannot connect to server \"%s:%d\"", Settings::getCamera_7IP().toStdString().c_str(), Settings::getCamera_8port()); cameraTcpClient_.close(); @@ -258,12 +174,12 @@ bool Camera::start() { images_.append(path.toStdString() + UDirectory::separator() + *iter); } - printf("Camera: Reading %d images from directory \"%s\"...\n", (int)images_.size(), path.toStdString().c_str()); + UINFO("Camera: Reading %d images from directory \"%s\"...", (int)images_.size(), path.toStdString().c_str()); if(images_.isEmpty()) { - printf("[WARNING] Camera: Directory \"%s\" is empty (no images matching the \"%s\" extensions). " + UWARN("Camera: Directory \"%s\" is empty (no images matching the \"%s\" extensions). " "If you want to disable loading automatically this directory, " - "clear the Camera/mediaPath parameter. By default, webcam will be used instead of the directory.\n", + "clear the Camera/mediaPath parameter. By default, webcam will be used instead of the directory.", path.toStdString().c_str(), ext.toStdString().c_str()); } @@ -274,11 +190,13 @@ bool Camera::start() capture_.open(path.toStdString().c_str()); if(!capture_.isOpened()) { - printf("[WARNING] Camera: Cannot open file \"%s\". If you want to disable loading automatically this video file, clear the Camera/mediaPath parameter. By default, webcam will be used instead of the file.\n", path.toStdString().c_str()); + UWARN("Camera: Cannot open file \"%s\". If you want to disable loading " + "automatically this video file, clear the Camera/mediaPath parameter. " + "By default, webcam will be used instead of the file.", path.toStdString().c_str()); } else { - printf("Camera: Reading from video file \"%s\"...\n", path.toStdString().c_str()); + UINFO("Camera: Reading from video file \"%s\"...", path.toStdString().c_str()); } } if(!capture_.isOpened() && images_.empty()) @@ -290,13 +208,13 @@ bool Camera::start() capture_.set(CV_CAP_PROP_FRAME_WIDTH, double(Settings::getCamera_2imageWidth())); capture_.set(CV_CAP_PROP_FRAME_HEIGHT, double(Settings::getCamera_3imageHeight())); } - printf("Camera: Reading from camera device %d...\n", Settings::getCamera_1deviceId()); + UINFO("Camera: Reading from camera device %d...", Settings::getCamera_1deviceId()); } } } if(!capture_.isOpened() && images_.empty() && !cameraTcpClient_.isOpen()) { - printf("[ERROR] Camera: Failed to open a capture object!\n"); + UERROR("Camera: Failed to open a capture object!"); return false; } diff --git a/src/Camera.h b/src/Camera.h index b4f9e44e..0174f732 100644 --- a/src/Camera.h +++ b/src/Camera.h @@ -9,25 +9,7 @@ #include #include #include -#include - -class CameraTcpClient : public QTcpSocket -{ - Q_OBJECT; -public: - CameraTcpClient(QObject * parent = 0); - cv::Mat getImage(); - int imagesBuffered() const {return images_.size();} - -private Q_SLOTS: - void readReceivedData(); - void displayError(QAbstractSocket::SocketError socketError); - void connectionLost(); - -private: - quint64 blockSize_; - QVector images_; -}; +#include "CameraTcpClient.h" class Camera : public QObject { Q_OBJECT diff --git a/src/CameraTcpClient.cpp b/src/CameraTcpClient.cpp new file mode 100644 index 00000000..8aa7e033 --- /dev/null +++ b/src/CameraTcpClient.cpp @@ -0,0 +1,95 @@ +/* + * CameraTcpClient.cpp + * + * Created on: 2014-07-31 + * Author: mathieu + */ + +#include "CameraTcpClient.h" +#include "Settings.h" +#include "utilite/ULogger.h" + +CameraTcpClient::CameraTcpClient(QObject *parent) : + QTcpSocket(parent), + blockSize_(0) +{ + connect(this, SIGNAL(readyRead()), this, SLOT(readReceivedData())); + connect(this, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(displayError(QAbstractSocket::SocketError))); + connect(this, SIGNAL(disconnected()), this, SLOT(connectionLost())); +} + +cv::Mat CameraTcpClient::getImage() +{ + cv::Mat img; + if(images_.size()) + { + // if queue changed after tcp connection ended with images still in the buffer + int queue = Settings::getCamera_9queueSize(); + while(queue > 0 && images_.size() > queue) + { + images_.pop_front(); + } + + img = images_.front(); + images_.pop_front(); + } + return img; +} + +void CameraTcpClient::readReceivedData() +{ + QDataStream in(this); + in.setVersion(QDataStream::Qt_4_0); + + if (blockSize_ == 0) + { + if (this->bytesAvailable() < (int)sizeof(quint64)) + { + return; + } + + in >> blockSize_; + } + + if (this->bytesAvailable() < (int)blockSize_) + { + return; + } + + std::vector buf(blockSize_); + in.readRawData((char*)buf.data(), blockSize_); + images_.push_back(cv::imdecode(buf, cv::IMREAD_UNCHANGED)); + int queue = Settings::getCamera_9queueSize(); + while(queue > 0 && images_.size() > queue) + { + images_.pop_front(); + } + blockSize_ = 0; +} + +void CameraTcpClient::displayError(QAbstractSocket::SocketError socketError) +{ + switch (socketError) + { + case QAbstractSocket::RemoteHostClosedError: + break; + case QAbstractSocket::HostNotFoundError: + UWARN("CameraTcp: Tcp error: The host was not found. Please " + "check the host name and port settings.\n"); + break; + case QAbstractSocket::ConnectionRefusedError: + UWARN("CameraTcp: The connection was refused by the peer. " + "Make sure your images server is running, " + "and check that the host name and port " + "settings are correct."); + break; + default: + UERROR("The following error occurred: %s.", this->errorString().toStdString().c_str()); + break; + } +} + +void CameraTcpClient::connectionLost() +{ + //printf("[WARNING] CameraTcp: Connection lost!\n"); +} diff --git a/src/CameraTcpClient.h b/src/CameraTcpClient.h new file mode 100644 index 00000000..76966fd0 --- /dev/null +++ b/src/CameraTcpClient.h @@ -0,0 +1,32 @@ +/* + * CameraTcpClient.h + * + * Created on: 2014-07-31 + * Author: mathieu + */ + +#ifndef CAMERATCPCLIENT_H_ +#define CAMERATCPCLIENT_H_ + +#include +#include + +class CameraTcpClient : public QTcpSocket +{ + Q_OBJECT; +public: + CameraTcpClient(QObject * parent = 0); + cv::Mat getImage(); + int imagesBuffered() const {return images_.size();} + +private Q_SLOTS: + void readReceivedData(); + void displayError(QAbstractSocket::SocketError socketError); + void connectionLost(); + +private: + quint64 blockSize_; + QVector images_; +}; + +#endif /* CAMERATCPCLIENT_H_ */ diff --git a/src/FindObject.cpp b/src/FindObject.cpp new file mode 100644 index 00000000..f6868e97 --- /dev/null +++ b/src/FindObject.cpp @@ -0,0 +1,939 @@ +/* + * FindObject.cpp + * + * Created on: 2014-07-30 + * Author: mathieu + */ + +#include "FindObject.h" +#include "ObjSignature.h" +#include "Settings.h" +#include "utilite/UDirectory.h" +#include "utilite/ULogger.h" +#include "Vocabulary.h" + +#include +#include +#include +#include +#include + +FindObject::FindObject(QObject * parent) : + QObject(parent), + vocabulary_(new Vocabulary()), + detector_(Settings::createKeypointDetector()), + extractor_(Settings::createDescriptorExtractor()), + minMatchedDistance_(-1), + maxMatchedDistance_(-1) +{ + Q_ASSERT(detector_ != 0 && extractor_ != 0); +} + +FindObject::~FindObject() { + delete detector_; + delete extractor_; + delete vocabulary_; + objectsDescriptors_.clear(); +} + +int FindObject::loadObjects(const QString & dirPath) +{ + int loadedObjects = 0; + QString formats = Settings::getGeneral_imageFormats().remove('*').remove('.'); + UDirectory dir(dirPath.toStdString(), formats.toStdString()); + if(dir.isValid()) + { + const std::list & names = dir.getFileNames(); // sorted in natural order + for(std::list::const_iterator iter=names.begin(); iter!=names.end(); ++iter) + { + this->addObject((dirPath.toStdString()+dir.separator()+*iter).c_str()); + } + if(names.size()) + { + this->updateObjects(); + this->updateVocabulary(); + } + loadedObjects = (int)names.size(); + } + return loadedObjects; +} + +const ObjSignature * FindObject::addObject(const QString & filePath) +{ + UINFO("Load file %s", filePath.toStdString().c_str()); + if(!filePath.isNull()) + { + cv::Mat img = cv::imread(filePath.toStdString().c_str(), cv::IMREAD_GRAYSCALE); + if(!img.empty()) + { + int id = 0; + QFileInfo file(filePath); + QStringList list = file.fileName().split('.'); + if(list.size()) + { + bool ok = false; + id = list.front().toInt(&ok); + if(ok && id>0) + { + if(objects_.contains(id)) + { + UWARN("Object %d already added, a new ID will be generated (new id=%d).", id, Settings::getGeneral_nextObjID()); + id = 0; + } + } + else + { + id = 0; + } + } + return this->addObject(img, id); + } + } + return 0; +} + +const ObjSignature * FindObject::addObject(const cv::Mat & image, int id) +{ + Q_ASSERT(id >= 0); + ObjSignature * s = new ObjSignature(id, image); + if(!this->addObject(s)) + { + delete s; + return 0; + } + return s; +} + +bool FindObject::addObject(ObjSignature * obj) +{ + Q_ASSERT(obj != 0 && obj->id() >= 0); + if(obj->id() && objects_.contains(obj->id())) + { + UERROR("object with id %d already added!", obj->id()); + return false; + } + else if(obj->id() == 0) + { + obj->setId(Settings::getGeneral_nextObjID()); + } + + Settings::setGeneral_nextObjID(obj->id()+1); + + objects_.insert(obj->id(), obj); + clearVocabulary(); + + return true; +} + +void FindObject::removeObject(int id) +{ + if(objects_.contains(id)) + { + delete objects_.value(id); + objects_.remove(id); + clearVocabulary(); + } +} + +void FindObject::removeAllObjects() +{ + qDeleteAll(objects_); + objects_.clear(); + clearVocabulary(); +} + +void FindObject::updateDetectorExtractor() +{ + delete detector_; + delete extractor_; + detector_ = Settings::createKeypointDetector(); + extractor_ = Settings::createDescriptorExtractor(); + Q_ASSERT(detector_ != 0 && extractor_ != 0); +} + +std::vector limitKeypoints(const std::vector & keypoints, int maxKeypoints) +{ + std::vector kptsKept; + if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints) + { + // Sort words by response + std::multimap reponseMap; // + for(unsigned int i = 0; i (fabs(keypoints[i].response), i)); + } + + // Remove them + std::multimap::reverse_iterator iter = reponseMap.rbegin(); + kptsKept.resize(maxKeypoints); + for(unsigned int k=0; k < kptsKept.size() && iter!=reponseMap.rend(); ++k, ++iter) + { + kptsKept[k] = keypoints[iter->second]; + } + } + else + { + kptsKept = keypoints; + } + return kptsKept; +} + +class ExtractFeaturesThread : public QThread +{ +public: + ExtractFeaturesThread(int objectId, const cv::Mat & image) : + objectId_(objectId), + image_(image) + { + + } + int objectId() const {return objectId_;} + const cv::Mat & image() const {return image_;} + const std::vector & keypoints() const {return keypoints_;} + const cv::Mat & descriptors() const {return descriptors_;} + +protected: + virtual void run() + { + QTime time; + time.start(); + UINFO("Extracting descriptors from object %d...", objectId_); + KeypointDetector * detector = Settings::createKeypointDetector(); + keypoints_.clear(); + descriptors_ = cv::Mat(); + detector->detect(image_, keypoints_); + delete detector; + + if(keypoints_.size()) + { + int maxFeatures = Settings::getFeature2D_3MaxFeatures(); + if(maxFeatures > 0 && (int)keypoints_.size() > maxFeatures) + { + int previousCount = (int)keypoints_.size(); + keypoints_ = limitKeypoints(keypoints_, maxFeatures); + UINFO("obj=%d, %d keypoints removed, (kept %d), min/max response=%f/%f", objectId_, previousCount-(int)keypoints_.size(), (int)keypoints_.size(), keypoints_.size()?keypoints_.back().response:0.0f, keypoints_.size()?keypoints_.front().response:0.0f); + } + + DescriptorExtractor * extractor = Settings::createDescriptorExtractor(); + extractor->compute(image_, keypoints_, descriptors_); + delete extractor; + if((int)keypoints_.size() != descriptors_.rows) + { + UERROR("obj=%d kpt=%d != descriptors=%d", objectId_, (int)keypoints_.size(), descriptors_.rows); + } + } + else + { + UWARN("no features detected in object %d !?!", objectId_); + } + UINFO("%d descriptors extracted from object %d (in %d ms)", descriptors_.rows, objectId_, time.elapsed()); + } +private: + int objectId_; + cv::Mat image_; + std::vector keypoints_; + cv::Mat descriptors_; +}; + +void FindObject::updateObjects() +{ + if(objects_.size()) + { + int threadCounts = Settings::getGeneral_threads(); + if(threadCounts == 0) + { + threadCounts = objects_.size(); + } + + QTime time; + time.start(); + UINFO("Features extraction from %d objects...", objects_.size()); + QList objectsList = objects_.values(); + for(int i=0; i threads; + for(int k=i; kid(), objectsList.at(k)->image())); + threads.back()->start(); + } + + for(int j=0; jwait(); + + int id = threads[j]->objectId(); + + objects_.value(id)->setData( + threads[j]->keypoints(), + threads[j]->descriptors(), + Settings::currentDetectorType(), + Settings::currentDescriptorType()); + } + } + UINFO("Features extraction from %d objects... done! (%d ms)", objects_.size(), time.elapsed()); + } + else + { + UINFO("No objects to update..."); + } +} + +void FindObject::clearVocabulary() +{ + objectsDescriptors_.clear(); + dataRange_.clear(); + vocabulary_->clear(); +} + +void FindObject::updateVocabulary() +{ + clearVocabulary(); + int count = 0; + int dim = -1; + int type = -1; + // Get the total size and verify descriptors + QList objectsList = objects_.values(); + for(int i=0; idescriptors().empty()) + { + if(dim >= 0 && objectsList.at(i)->descriptors().cols != dim) + { + UERROR("Descriptors of the objects are not all the same size! Objects " + "opened must have all the same size (and from the same descriptor extractor)."); + return; + } + dim = objectsList.at(i)->descriptors().cols; + if(type >= 0 && objectsList.at(i)->descriptors().type() != type) + { + UERROR("Descriptors of the objects are not all the same type! Objects opened " + "must have been processed by the same descriptor extractor."); + return; + } + type = objectsList.at(i)->descriptors().type(); + count += objectsList.at(i)->descriptors().rows; + } + } + + // Copy data + if(count) + { + UINFO("Updating global descriptors matrix: Objects=%d, total descriptors=%d, dim=%d, type=%d", + (int)objects_.size(), count, dim, type); + if(Settings::getGeneral_invertedSearch() || Settings::getGeneral_threads() == 1) + { + // If only one thread, put all descriptors in the same cv::Mat + objectsDescriptors_.insert(0, cv::Mat(count, dim, type)); + int row = 0; + for(int i=0; idescriptors().rows) + { + cv::Mat dest(objectsDescriptors_.begin().value(), cv::Range(row, row+objectsList.at(i)->descriptors().rows)); + objectsList.at(i)->descriptors().copyTo(dest); + row += objectsList.at(i)->descriptors().rows; + // dataRange contains the upper_bound for each + // object (the last descriptors position in the + // global object descriptors matrix) + if(objectsList.at(i)->descriptors().rows) + { + dataRange_.insert(row-1, objectsList.at(i)->id()); + } + } + } + + if(Settings::getGeneral_invertedSearch()) + { + QTime time; + time.start(); + bool incremental = Settings::getGeneral_vocabularyIncremental(); + if(incremental) + { + UINFO("Creating incremental vocabulary..."); + } + else + { + UINFO("Creating vocabulary..."); + } + QTime localTime; + localTime.start(); + int updateVocabularyMinWords = Settings::getGeneral_vocabularyUpdateMinWords(); + int addedWords = 0; + for(int i=0; i words = vocabulary_->addWords(objectsList[i]->descriptors(), objectsList.at(i)->id(), incremental); + objectsList[i]->setWords(words); + addedWords += words.uniqueKeys().size(); + bool updated = false; + if(incremental && addedWords && addedWords >= updateVocabularyMinWords) + { + vocabulary_->update(); + addedWords = 0; + updated = true; + } + UINFO("Object %d, %d words from %d descriptors (%d words, %d ms) %s", + objectsList[i]->id(), + words.uniqueKeys().size(), + objectsList[i]->descriptors().rows, + vocabulary_->size(), + localTime.restart(), + updated?"updated":""); + } + if(addedWords) + { + vocabulary_->update(); + } + + if(incremental) + { + UINFO("Creating incremental vocabulary... done! size=%d (%d ms)", vocabulary_->size(), time.elapsed()); + } + else + { + UINFO("Creating vocabulary... done! size=%d (%d ms)", vocabulary_->size(), time.elapsed()); + } + } + } + else + { + for(int i=0; iid(), objectsList.at(i)->descriptors()); + } + } + } +} + +class SearchThread: public QThread +{ +public: + SearchThread(Vocabulary * vocabulary, int objectId, const cv::Mat * descriptors, const QMultiMap * sceneWords) : + vocabulary_(vocabulary), + objectId_(objectId), + descriptors_(descriptors), + sceneWords_(sceneWords), + minMatchedDistance_(-1.0f), + maxMatchedDistance_(-1.0f) + { + Q_ASSERT(index && descriptors); + } + virtual ~SearchThread() {} + + int getObjectId() const {return objectId_;} + float getMinMatchedDistance() const {return minMatchedDistance_;} + float getMaxMatchedDistance() const {return maxMatchedDistance_;} + const QMultiMap & getMatches() const {return matches_;} + +protected: + virtual void run() + { + //QTime time; + //time.start(); + + cv::Mat results; + cv::Mat dists; + + //match objects to scene + int k = Settings::getNearestNeighbor_3nndrRatioUsed()?2:1; + results = cv::Mat(descriptors_->rows, k, CV_32SC1); // results index + dists = cv::Mat(descriptors_->rows, k, CV_32FC1); // Distance results are CV_32FC1 + vocabulary_->search(*descriptors_, results, dists, k); + + // PROCESS RESULTS + // Get all matches for each object + for(int i=0; i(i,0) <= Settings::getNearestNeighbor_4nndrRatio() * dists.at(i,1)) + { + matched = true; + } + if((matched || !Settings::getNearestNeighbor_3nndrRatioUsed()) && + Settings::getNearestNeighbor_5minDistanceUsed()) + { + if(dists.at(i,0) <= Settings::getNearestNeighbor_6minDistance()) + { + matched = true; + } + else + { + matched = false; + } + } + if(!matched && !Settings::getNearestNeighbor_3nndrRatioUsed() && !Settings::getNearestNeighbor_5minDistanceUsed()) + { + matched = true; // no criterion, match to the nearest descriptor + } + if(minMatchedDistance_ == -1 || minMatchedDistance_ > dists.at(i,0)) + { + minMatchedDistance_ = dists.at(i,0); + } + if(maxMatchedDistance_ == -1 || maxMatchedDistance_ < dists.at(i,0)) + { + maxMatchedDistance_ = dists.at(i,0); + } + + int wordId = results.at(i,0); + if(matched && sceneWords_->count(wordId) == 1) + { + matches_.insert(i, sceneWords_->value(wordId)); + matches_.insert(i, results.at(i,0)); + } + } + + //UINFO("Search Object %d time=%d ms", objectIndex_, time.elapsed()); + } +private: + Vocabulary * vocabulary_; // would be const but flann search() method is not const!? + int objectId_; + const cv::Mat * descriptors_; + const QMultiMap * sceneWords_; // + + float minMatchedDistance_; + float maxMatchedDistance_; + QMultiMap matches_; +}; + +class HomographyThread: public QThread +{ +public: + HomographyThread( + const QMultiMap * matches, // + int objectId, + const std::vector * kptsA, + const std::vector * kptsB) : + matches_(matches), + objectId_(objectId), + kptsA_(kptsA), + kptsB_(kptsB) + { + Q_ASSERT(matches && kptsA && kptsB); + } + virtual ~HomographyThread() {} + + int getObjectId() const {return objectId_;} + const std::vector & getIndexesA() const {return indexesA_;} + const std::vector & getIndexesB() const {return indexesB_;} + const std::vector & getOutlierMask() const {return outlierMask_;} + QMultiMap getInliers() const {return inliers_;} + QMultiMap getOutliers() const {return outliers_;} + const cv::Mat & getHomography() const {return h_;} + +protected: + virtual void run() + { + //QTime time; + //time.start(); + + std::vector mpts_1(matches_->size()); + std::vector mpts_2(matches_->size()); + indexesA_.resize(matches_->size()); + indexesB_.resize(matches_->size()); + + int j=0; + for(QMultiMap::const_iterator iter = matches_->begin(); iter!=matches_->end(); ++iter) + { + mpts_1[j] = kptsA_->at(iter.key()).pt; + indexesA_[j] = iter.key(); + mpts_2[j] = kptsB_->at(iter.value()).pt; + indexesB_[j] = iter.value(); + ++j; + } + + if((int)mpts_1.size() >= Settings::getHomography_minimumInliers()) + { + h_ = findHomography(mpts_1, + mpts_2, + Settings::getHomographyMethod(), + Settings::getHomography_ransacReprojThr(), + outlierMask_); + + for(unsigned int k=0; k * matches_; + int objectId_; + const std::vector * kptsA_; + const std::vector * kptsB_; + + std::vector indexesA_; + std::vector indexesB_; + std::vector outlierMask_; + QMultiMap inliers_; + QMultiMap outliers_; + cv::Mat h_; +}; + +void FindObject::detect(const cv::Mat & image) +{ + QMultiMap > objects; + this->detect(image, objects); + if(objects.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents()) + { + Q_EMIT objectsFound(objects); + } + + if(objects.size() > 1) + { + UINFO("(%s) %d objects detected!", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), + (int)objects.size()); + } + else if(objects.size() == 1) + { + UINFO("(%s) Object %d detected!", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), + (int)objects.begin().key()); + } + else if(Settings::getGeneral_sendNoObjDetectedEvents()) + { + UINFO("(%s) No objects detected.", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str()); + } +} + +bool FindObject::detect(const cv::Mat & image, QMultiMap > & objectsDetected) +{ + QTime totalTime; + totalTime.start(); + + // reset statistics + timeStamps_.clear(); + sceneKeypoints_.clear(); + sceneDescriptors_ = cv::Mat(); + sceneWords_.clear(); + matches_.clear(); + inliers_.clear(); + outliers_.clear(); + minMatchedDistance_ = -1.0f; + maxMatchedDistance_ = -1.0f; + + bool success = false; + if(!image.empty()) + { + //Convert to grayscale + cv::Mat grayscaleImg; + if(image.channels() != 1 || image.depth() != CV_8U) + { + cv::cvtColor(image, grayscaleImg, cv::COLOR_BGR2GRAY); + } + else + { + grayscaleImg = image; + } + + QTime time; + time.start(); + + // EXTRACT KEYPOINTS + detector_->detect(grayscaleImg, sceneKeypoints_); + timeStamps_.insert(kTimeKeypointDetection, time.restart()); + + if(sceneKeypoints_.size()) + { + int maxFeatures = Settings::getFeature2D_3MaxFeatures(); + if(maxFeatures > 0 && (int)sceneKeypoints_.size() > maxFeatures) + { + sceneKeypoints_ = limitKeypoints(sceneKeypoints_, maxFeatures); + } + + // EXTRACT DESCRIPTORS + extractor_->compute(grayscaleImg, sceneKeypoints_, sceneDescriptors_); + if((int)sceneKeypoints_.size() != sceneDescriptors_.rows) + { + UERROR("kpt=%d != descriptors=%d\n", (int)sceneKeypoints_.size(), sceneDescriptors_.rows); + } + } + else + { + UWARN("no features detected !?!\n"); + } + timeStamps_.insert(kTimeDescriptorExtraction, time.restart()); + + bool consistentNNData = (vocabulary_->size()!=0 && vocabulary_->wordToObjects().begin().value()!=-1 && Settings::getGeneral_invertedSearch()) || + ((vocabulary_->size()==0 || vocabulary_->wordToObjects().begin().value()==-1) && !Settings::getGeneral_invertedSearch()); + + // COMPARE + if(!objectsDescriptors_.empty() && + sceneKeypoints_.size() && + consistentNNData && + objectsDescriptors_.begin().value().cols == sceneDescriptors_.cols && + objectsDescriptors_.begin().value().type() == sceneDescriptors_.type()) // binary descriptor issue, if the dataTree is not yet updated with modified settings + { + success = true; + + QMultiMap words; + + if(!Settings::getGeneral_invertedSearch()) + { + // CREATE INDEX for the scene + vocabulary_->clear(); + words = vocabulary_->addWords(sceneDescriptors_, -1, Settings::getGeneral_vocabularyIncremental()); + if(!Settings::getGeneral_vocabularyIncremental()) + { + vocabulary_->update(); + } + timeStamps_.insert(kTimeIndexing, time.restart()); + } + + for(QMap::iterator iter=objects_.begin(); iter!=objects_.end(); ++iter) + { + matches_.insert(iter.key(), QMultiMap()); + } + + if(Settings::getGeneral_invertedSearch() || Settings::getGeneral_threads() == 1) + { + cv::Mat results; + cv::Mat dists; + // DO NEAREST NEIGHBOR + int k = Settings::getNearestNeighbor_3nndrRatioUsed()?2:1; + if(!Settings::getGeneral_invertedSearch()) + { + //match objects to scene + results = cv::Mat(objectsDescriptors_.begin().value().rows, k, CV_32SC1); // results index + dists = cv::Mat(objectsDescriptors_.begin().value().rows, k, CV_32FC1); // Distance results are CV_32FC1 + vocabulary_->search(objectsDescriptors_.begin().value(), results, dists, k); + } + else + { + //match scene to objects + results = cv::Mat(sceneDescriptors_.rows, k, CV_32SC1); // results index + dists = cv::Mat(sceneDescriptors_.rows, k, CV_32FC1); // Distance results are CV_32FC1 + vocabulary_->search(sceneDescriptors_, results, dists, k); + } + + // PROCESS RESULTS + // Get all matches for each object + for(int i=0; i(i,0) <= Settings::getNearestNeighbor_4nndrRatio() * dists.at(i,1)) + { + matched = true; + } + if((matched || !Settings::getNearestNeighbor_3nndrRatioUsed()) && + Settings::getNearestNeighbor_5minDistanceUsed()) + { + if(dists.at(i,0) <= Settings::getNearestNeighbor_6minDistance()) + { + matched = true; + } + else + { + matched = false; + } + } + if(!matched && !Settings::getNearestNeighbor_3nndrRatioUsed() && !Settings::getNearestNeighbor_5minDistanceUsed()) + { + matched = true; // no criterion, match to the nearest descriptor + } + if(minMatchedDistance_ == -1 || minMatchedDistance_ > dists.at(i,0)) + { + minMatchedDistance_ = dists.at(i,0); + } + if(maxMatchedDistance_ == -1 || maxMatchedDistance_ < dists.at(i,0)) + { + maxMatchedDistance_ = dists.at(i,0); + } + + if(matched) + { + if(Settings::getGeneral_invertedSearch()) + { + int wordId = results.at(i,0); + QList objIds = vocabulary_->wordToObjects().values(wordId); + for(int j=0; jwordToObjects().count(wordId, objIds[j]) == 1) + { + matches_.find(objIds[j]).value().insert(objects_.value(objIds[j])->words().value(wordId), i); + } + } + } + else + { + QMap::iterator iter = dataRange_.lowerBound(i); + int objectId = iter.value(); + int fisrtObjectDescriptorIndex = (iter == dataRange_.begin())?0:(--iter).key()+1; + int objectDescriptorIndex = i - fisrtObjectDescriptorIndex; + + int wordId = results.at(i,0); + if(words.count(wordId) == 1) + { + matches_.find(objectId).value().insert(objectDescriptorIndex, words.value(wordId)); + } + } + } + } + } + else + { + //multi-threaded, match objects to scene + int threadCounts = Settings::getGeneral_threads(); + if(threadCounts == 0) + { + threadCounts = (int)objectsDescriptors_.size(); + } + + QList objectsDescriptorsId = objectsDescriptors_.keys(); + QList objectsDescriptorsMat = objectsDescriptors_.values(); + for(int j=0; j threads; + + for(int k=j; kstart(); + } + + for(int k=0; kwait(); + matches_[threads[k]->getObjectId()] = threads[k]->getMatches(); + + if(minMatchedDistance_ == -1 || minMatchedDistance_ > threads[k]->getMinMatchedDistance()) + { + minMatchedDistance_ = threads[k]->getMinMatchedDistance(); + } + if(maxMatchedDistance_ == -1 || maxMatchedDistance_ < threads[k]->getMaxMatchedDistance()) + { + maxMatchedDistance_ = threads[k]->getMaxMatchedDistance(); + } + delete threads[k]; + } + + } + } + + timeStamps_.insert(kTimeMatching, time.restart()); + + // Homographies + if(Settings::getHomography_homographyComputed()) + { + // HOMOGRAPHY + int threadCounts = Settings::getGeneral_threads(); + if(threadCounts == 0) + { + threadCounts = matches_.size(); + } + QList matchesId = matches_.keys(); + QList > matchesList = matches_.values(); + for(int i=0; i threads; + + for(int k=i; kkeypoints(), &sceneKeypoints_)); + threads.back()->start(); + } + + for(int j=0; jwait(); + + int id = threads[j]->getObjectId(); + + if(!threads[j]->getHomography().empty()) + { + if(threads[j]->getInliers().size() >= Settings::getHomography_minimumInliers()) + { + const cv::Mat & H = threads[j]->getHomography(); + QTransform hTransform( + H.at(0,0), H.at(1,0), H.at(2,0), + H.at(0,1), H.at(1,1), H.at(2,1), + H.at(0,2), H.at(1,2), H.at(2,2)); + + int distance = Settings::getGeneral_multiDetectionRadius(); // in pixels + if(Settings::getGeneral_multiDetection()) + { + // Get the outliers and recompute homography with them + matchesList.push_back(threads[j]->getOutliers()); + matchesId.push_back(id); + + // compute distance from previous added same objects... + QMultiMap >::iterator objIter = objectsDetected.find(id); + for(;objIter!=objectsDetected.end() && objIter.key() == id; ++objIter) + { + qreal dx = objIter.value().second.m31() - hTransform.m31(); + qreal dy = objIter.value().second.m32() - hTransform.m32(); + int d = (int)sqrt(dx*dx + dy*dy); + if(d < distance) + { + distance = d; + } + } + } + + if(distance >= Settings::getGeneral_multiDetectionRadius()) + { + QRect rect = objects_.value(id)->rect(); + objectsDetected.insert(id, QPair(rect, hTransform)); + inliers_.insert(id, threads[j]->getInliers()); + outliers_.insert(id, threads[j]->getOutliers()); + } + else + { + rejectedInliers_.insert(id, threads[j]->getInliers()); + rejectedOutliers_.insert(id, threads[j]->getOutliers()); + } + } + else + { + rejectedInliers_.insert(id, threads[j]->getInliers()); + rejectedOutliers_.insert(id, threads[j]->getOutliers()); + } + } + else + { + rejectedInliers_.insert(id, threads[j]->getInliers()); + rejectedOutliers_.insert(id, threads[j]->getOutliers()); + } + } + } + timeStamps_.insert(kTimeHomography, time.restart()); + } + } + else if(!objectsDescriptors_.empty() && sceneKeypoints_.size()) + { + UWARN("Cannot search, objects must be updated"); + } + } + + timeStamps_.insert(kTimeTotal, totalTime.elapsed()); + + return success; +} diff --git a/src/FindObject.h b/src/FindObject.h new file mode 100644 index 00000000..715be113 --- /dev/null +++ b/src/FindObject.h @@ -0,0 +1,96 @@ +/* + * FindObject.h + * + * Created on: 2014-07-30 + * Author: mathieu + */ + +#ifndef FINDOBJECT_H_ +#define FINDOBJECT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ObjSignature; +class Vocabulary; +class KeypointDetector; +class DescriptorExtractor; + +class FindObject : public QObject +{ + Q_OBJECT; + +public: + enum TimeStamp{kTimeKeypointDetection, kTimeDescriptorExtraction, kTimeIndexing, kTimeMatching, kTimeHomography, kTimeTotal}; + +public: + FindObject(QObject * parent = 0); + virtual ~FindObject(); + + int loadObjects(const QString & dirPath); // call updateObjects() + const ObjSignature * addObject(const QString & filePath); + const ObjSignature * addObject(const cv::Mat & image, int id=0); + bool addObject(ObjSignature * obj); // take ownership when true is returned + void removeObject(int id); + void removeAllObjects(); + + bool detect(const cv::Mat & image, QMultiMap > & objectsDetected); + + void updateDetectorExtractor(); + void updateObjects(); + void updateVocabulary(); + + const QMap & objects() const {return objects_;} + const Vocabulary * vocabulary() const {return vocabulary_;} + + const QMap & timeStamps() const {return timeStamps_;} + const std::vector & sceneKeypoints() const {return sceneKeypoints_;} + const cv::Mat & sceneDescriptors() const {return sceneDescriptors_;} + const QMultiMap & sceneWords() const {return sceneWords_;} + const QMap > & matches() const {return matches_;} + const QMultiMap > & inliers() const {return inliers_;} + const QMultiMap > & outliers() const {return outliers_;} + const QMultiMap > & rejectedInliers() const {return rejectedInliers_;} + const QMultiMap > & rejectedOutliers() const {return rejectedOutliers_;} + float minMatchedDistance() const {return minMatchedDistance_;} + float maxMatchedDistance() const {return maxMatchedDistance_;} + +public Q_SLOTS: + void detect(const cv::Mat & image); + +Q_SIGNALS: + void objectsFound(const QMultiMap > &); + +private: + void clearVocabulary(); + +private: + QMap objects_; + Vocabulary * vocabulary_; + QMap objectsDescriptors_; + QMap dataRange_; // + KeypointDetector * detector_; + DescriptorExtractor * extractor_; + + QMap timeStamps_; + std::vector sceneKeypoints_; + cv::Mat sceneDescriptors_; + QMultiMap sceneWords_; + QMap > matches_; // ObjectID Map< ObjectDescriptorIndex, SceneDescriptorIndex >, match the number of objects + QMultiMap > inliers_; // ObjectID Map< ObjectDescriptorIndex, SceneDescriptorIndex >, match the number of detected objects + QMultiMap > outliers_; // ObjectID Map< ObjectDescriptorIndex, SceneDescriptorIndex >, match the number of detected objects + QMultiMap > rejectedInliers_; // ObjectID Map< ObjectDescriptorIndex, SceneDescriptorIndex > + QMultiMap > rejectedOutliers_; // ObjectID Map< ObjectDescriptorIndex, SceneDescriptorIndex > + float minMatchedDistance_; + float maxMatchedDistance_; +}; + +#endif /* FINDOBJECT_H_ */ diff --git a/src/MainWindow.cpp b/src/MainWindow.cpp index 4a05e04f..115bb6d4 100644 --- a/src/MainWindow.cpp +++ b/src/MainWindow.cpp @@ -16,6 +16,8 @@ #include "TcpServer.h" #include "rtabmap/PdfPlot.h" #include "Vocabulary.h" +#include "FindObject.h" +#include "ObjSignature.h" #include #include @@ -42,21 +44,21 @@ #include #include "utilite/UDirectory.h" +#include "utilite/ULogger.h" // Camera ownership transferred -MainWindow::MainWindow(Camera * camera, const QString & settings, QWidget * parent) : +MainWindow::MainWindow(FindObject * findObject, Camera * camera, QWidget * parent) : QMainWindow(parent), camera_(camera), - settings_(settings), + findObject_(findObject), likelihoodCurve_(0), inliersCurve_(0), - vocabulary_(new Vocabulary()), lowestRefreshRate_(99), objectsModified_(false), - tcpServer_(0), - detector_(0), - extractor_(0) + tcpServer_(0) { + Q_ASSERT(findObject_ != 0); + ui_ = new Ui_mainWindow(); ui_->setupUi(this); aboutDialog_ = new AboutDialog(this); @@ -75,14 +77,9 @@ MainWindow::MainWindow(Camera * camera, const QString & settings, QWidget * pare ui_->dockWidget_plot->setVisible(false); ui_->widget_controls->setVisible(false); - if(settings_.isEmpty()) - { - settings_ = Settings::iniDefaultPath(); - } - QByteArray geometry; QByteArray state; - Settings::loadSettings(settings_, &geometry, &state); + Settings::loadWindowSettings(geometry, state); this->restoreGeometry(geometry); this->restoreState(state); lastObjectsUpdateParameters_ = Settings::getParameters(); @@ -111,9 +108,6 @@ MainWindow::MainWindow(Camera * camera, const QString & settings, QWidget * pare if(cv::gpu::getCudaEnabledDeviceCount() == 0) { - Settings::setFeature2D_SURF_gpu(false); - Settings::setFeature2D_Fast_gpu(false); - Settings::setFeature2D_ORB_gpu(false); ui_->toolBox->updateParameter(Settings::kFeature2D_SURF_gpu()); ui_->toolBox->updateParameter(Settings::kFeature2D_Fast_gpu()); ui_->toolBox->updateParameter(Settings::kFeature2D_ORB_gpu()); @@ -124,10 +118,6 @@ MainWindow::MainWindow(Camera * camera, const QString & settings, QWidget * pare ui_->toolBox->getParameterWidget(Settings::kFeature2D_ORB_gpu())->setEnabled(false); } - detector_ = Settings::createKeypointDetector(); - extractor_ = Settings::createDescriptorExtractor(); - Q_ASSERT(detector_ != 0 && extractor_ != 0); - connect((QDoubleSpinBox*)ui_->toolBox->getParameterWidget(Settings::kCamera_4imageRate()), SIGNAL(editingFinished()), camera_, @@ -206,6 +196,20 @@ MainWindow::MainWindow(Camera * camera, const QString & settings, QWidget * pare ui_->label_port->setTextInteractionFlags(Qt::TextSelectableByMouse); setupTCPServer(); + if(findObject_->objects().size()) + { + // show objects already loaded in FindObject + for(QMap::const_iterator iter = findObject_->objects().constBegin(); + iter!=findObject_->objects().constEnd(); + ++iter) + { + ObjWidget * obj = new ObjWidget(iter.key(), iter.value()->keypoints(), cvtCvMat2QImage(iter.value()->image())); + objWidgets_.insert(obj->id(), obj); + this->showObject(obj); + } + } + + if(Settings::getGeneral_autoStartCamera()) { // Set 1 msec to see state on the status bar. @@ -217,20 +221,17 @@ MainWindow::~MainWindow() { disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &))); camera_->stop(); - delete detector_; - delete extractor_; - delete vocabulary_; - objectsDescriptors_.clear(); - qDeleteAll(objects_.begin(), objects_.end()); - objects_.clear(); + qDeleteAll(objWidgets_); + objWidgets_.clear(); delete ui_; + delete findObject_; } void MainWindow::closeEvent(QCloseEvent * event) { bool quit = true; this->stopProcessing(); - if(objectsModified_ && this->isVisible() && objects_.size()) + if(objectsModified_ && this->isVisible() && objWidgets_.size()) { int ret = QMessageBox::question(this, tr("Save new objects"), tr("Do you want to save added objects?"), QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); switch(ret) @@ -248,7 +249,7 @@ void MainWindow::closeEvent(QCloseEvent * event) } if(quit) { - Settings::saveSettings(settings_, this->saveGeometry(), this->saveState()); + Settings::saveWindowSettings(this->saveGeometry(), this->saveState()); event->accept(); } else @@ -268,8 +269,7 @@ void MainWindow::setupTCPServer() connect(this, SIGNAL(objectsFound(QMultiMap >)), tcpServer_, SLOT(publishObjects(QMultiMap >))); ui_->label_ipAddress->setText(tcpServer_->getHostAddress().toString()); ui_->label_port->setNum(tcpServer_->getPort()); - printf("IP: %s\nport: %d\n", - tcpServer_->getHostAddress().toString().toStdString().c_str(), tcpServer_->getPort()); + UINFO("IP: %s port: %d", tcpServer_->getHostAddress().toString().toStdString().c_str(), tcpServer_->getPort()); } void MainWindow::setSourceImageText(const QString & text) @@ -308,7 +308,8 @@ bool MainWindow::loadSettings(const QString & path) { QByteArray geometry; QByteArray state; - Settings::loadSettings(path, &geometry, &state); + Settings::loadSettings(path); + Settings::loadWindowSettings(geometry, state, path); this->restoreGeometry(geometry); this->restoreState(state); @@ -321,7 +322,7 @@ bool MainWindow::loadSettings(const QString & path) return true; } - printf("Path \"%s\" not valid (should be *.ini)\n", path.toStdString().c_str()); + UINFO("Path \"%s\" not valid (should be *.ini)", path.toStdString().c_str()); return false; } @@ -329,10 +330,11 @@ bool MainWindow::saveSettings(const QString & path) { if(!path.isEmpty() && QFileInfo(path).suffix().compare("ini") == 0) { - Settings::saveSettings(path, this->saveGeometry(), this->saveState()); + Settings::saveSettings(path); + Settings::saveWindowSettings(this->saveGeometry(), this->saveState(), path); return true; } - printf("Path \"%s\" not valid (should be *.ini)\n", path.toStdString().c_str()); + UINFO("Path \"%s\" not valid (should be *.ini)", path.toStdString().c_str()); return false; } @@ -346,28 +348,39 @@ int MainWindow::loadObjects(const QString & dirPath) const std::list & names = dir.getFileNames(); // sorted in natural order for(std::list::const_iterator iter=names.begin(); iter!=names.end(); ++iter) { - this->addObjectFromFile((dirPath.toStdString()+dir.separator()+*iter).c_str()); + if(this->addObjectFromFile((dirPath.toStdString()+dir.separator()+*iter).c_str())) + { + ++loadedObjects; + } } - if(names.size()) + if(loadedObjects) { this->updateObjects(); } - loadedObjects = (int)names.size(); } return loadedObjects; } -void MainWindow::saveObjects(const QString & dirPath) +int MainWindow::saveObjects(const QString & dirPath) { + int count = 0; QDir dir(dirPath); if(dir.exists()) { - for(int i=0; i::iterator iter=objWidgets_.begin(); iter!=objWidgets_.end(); ++iter) { - objects_.at(i)->pixmap().save(QString("%1/%2.png").arg(dirPath).arg(objects_.at(i)->id())); + if(iter.value()->pixmap().save(QString("%1/%2.png").arg(dirPath).arg(iter.key()))) + { + ++count; + } + else + { + UERROR("Failed to save object %d", iter.key()); + } } objectsModified_ = false; } + return count; } void MainWindow::loadObjects() @@ -375,7 +388,15 @@ void MainWindow::loadObjects() QString dirPath = QFileDialog::getExistingDirectory(this, tr("Loading objects... Select a directory."), Settings::workingDirectory()); if(!dirPath.isEmpty()) { - loadObjects(dirPath); + int count = loadObjects(dirPath); + if(count) + { + QMessageBox::information(this, tr("Loading..."), tr("%1 objects loaded from \"%2\".").arg(count).arg(dirPath)); + } + else + { + QMessageBox::information(this, tr("Loading..."), tr("No objects loaded from \"%1\"!").arg(dirPath)); + } } } bool MainWindow::saveObjects() @@ -383,8 +404,16 @@ bool MainWindow::saveObjects() QString dirPath = QFileDialog::getExistingDirectory(this, tr("Saving objects... Select a directory."), Settings::workingDirectory()); if(!dirPath.isEmpty()) { - saveObjects(dirPath); - return true; + int count = saveObjects(dirPath); + if(count) + { + QMessageBox::information(this, tr("Saving..."), tr("%1 objects saved to \"%2\".").arg(count).arg(dirPath)); + } + else + { + QMessageBox::warning(this, tr("Saving..."), tr("No objects saved to %1!").arg(dirPath)); + } + return count > 0; } return false; } @@ -393,35 +422,37 @@ void MainWindow::removeObject(ObjWidget * object) { if(object) { - objects_.removeOne(object); - object->deleteLater(); - this->updateData(); - if(!camera_->isRunning() && !ui_->imageView_source->cvImage().empty()) + objWidgets_.remove(object->id()); + if(objWidgets_.size() == 0) { - this->update(ui_->imageView_source->cvImage()); + ui_->actionSave_objects->setEnabled(false); + } + findObject_->removeObject(object->id()); + object->deleteLater(); + if(!camera_->isRunning() && !sceneImage_.empty()) + { + this->update(); } } } void MainWindow::removeAllObjects() { - for(int i=0; iactionSave_objects->setEnabled(false); + findObject_->removeAllObjects(); + if(!camera_->isRunning() && !sceneImage_.empty()) { - delete objects_.at(i); - } - objects_.clear(); - this->updateData(); - if(!camera_->isRunning() && !ui_->imageView_source->cvImage().empty()) - { - this->update(ui_->imageView_source->cvImage()); + this->update(); } } void MainWindow::updateObjectsSize() { - for(int i=0; i::iterator iter=objWidgets_.begin(); iter!=objWidgets_.end(); ++iter) { - updateObjectSize(objects_[i]); + updateObjectSize(iter.value()); } } @@ -446,9 +477,9 @@ void MainWindow::updateMirrorView() { bool mirrorView = Settings::getGeneral_mirrorView(); ui_->imageView_source->setMirrorView(mirrorView); - for(int i=0; i::iterator iter=objWidgets_.begin(); iter!=objWidgets_.end(); ++iter) { - objects_[i]->setMirrorView(mirrorView); + iter.value()->setMirrorView(mirrorView); } } @@ -462,33 +493,38 @@ void MainWindow::addObjectFromScene() disconnect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &))); AddObjectDialog * dialog; bool resumeCamera = camera_->isRunning(); - if(camera_->isRunning() || ui_->imageView_source->cvImage().empty()) + if(camera_->isRunning() || sceneImage_.empty()) { dialog = new AddObjectDialog(camera_, cv::Mat(), ui_->imageView_source->isMirrorView(), this); } else { - dialog = new AddObjectDialog(0, ui_->imageView_source->cvImage(), ui_->imageView_source->isMirrorView(), this); + dialog = new AddObjectDialog(0, sceneImage_, ui_->imageView_source->isMirrorView(), this); } if(dialog->exec() == QDialog::Accepted) { - ObjWidget * obj = dialog->retrieveObject(); - if(obj) - { - objects_.push_back(obj); - showObject(objects_.last()); - updateData(); - objectsModified_ = true; - } + ObjWidget * obj = 0; + ObjSignature * signature = 0; + dialog->retrieveObject(&obj, &signature); + Q_ASSERT(obj!=0 && signature!=0); + findObject_->addObject(signature); + obj->setId(signature->id()); + objWidgets_.insert(obj->id(), obj); + ui_->actionSave_objects->setEnabled(true); + showObject(obj); + QLabel * detectorDescriptorType = qFindChild(this, QString("%1type").arg(obj->id())); + detectorDescriptorType->setText(QString("%1/%2").arg(signature->detectorType()).arg(signature->descriptorType())); + updateVocabulary(); + objectsModified_ = true; } - if(resumeCamera || ui_->imageView_source->cvImage().empty()) + if(resumeCamera || sceneImage_.empty()) { this->startProcessing(); } else { connect(camera_, SIGNAL(imageReceived(const cv::Mat &)), this, SLOT(update(const cv::Mat &)), Qt::UniqueConnection); - this->update(ui_->imageView_source->cvImage()); + this->update(); } delete dialog; } @@ -507,48 +543,21 @@ void MainWindow::addObjectsFromFiles() } } -void MainWindow::addObjectFromFile(const QString & filePath) +bool MainWindow::addObjectFromFile(const QString & filePath) { - printf("Load file %s\n", filePath.toStdString().c_str()); - if(!filePath.isNull()) + const ObjSignature * s = findObject_->addObject(filePath); + if(s) { - cv::Mat img = cv::imread(filePath.toStdString().c_str(), cv::IMREAD_GRAYSCALE); - if(!img.empty()) - { - int id = 0; - QFileInfo file(filePath); - QStringList list = file.fileName().split('.'); - if(list.size()) - { - bool ok = false; - id = list.front().toInt(&ok); - if(ok) - { - for(int i=0; iid() == id) - { - if(this->isVisible()) - { - QMessageBox::warning(this, tr("Warning"), tr("Object %1 already added, a new ID will be generated.").arg(id)); - } - else - { - printf("WARNING: Object %d already added, a new ID will be generated.", id); - } - id = 0; - break; - } - } - } - else - { - id = 0; - } - } - objects_.append(new ObjWidget(id, std::vector(), cv::Mat(), img, "", "")); - this->showObject(objects_.last()); - } + ObjWidget * obj = new ObjWidget(s->id(), std::vector(), cvtCvMat2QImage(s->image())); + objWidgets_.insert(obj->id(), obj); + ui_->actionSave_objects->setEnabled(true); + this->showObject(obj); + return true; + } + else + { + QMessageBox::critical(this, tr("Error adding object"), tr("Failed to add object from \"%1\"").arg(filePath)); + return false; } } @@ -665,21 +674,10 @@ void MainWindow::showObject(ObjWidget * obj) obj->setMirrorView(ui_->imageView_source->isMirrorView()); QList objs = ui_->objects_area->findChildren(); QVBoxLayout * vLayout = new QVBoxLayout(); - int id = Settings::getGeneral_nextObjID(); - if(obj->id() == 0) - { - obj->setId(id++); - Settings::setGeneral_nextObjID(id); - } - else if(obj->id() > id) - { - id = obj->id()+1; - Settings::setGeneral_nextObjID(id); - } ui_->toolBox->updateParameter(Settings::kGeneral_nextObjID()); - QLabel * title = new QLabel(QString("%1 (%2)").arg(obj->id()).arg(QString::number(obj->keypoints().size())), this); - QLabel * detectorDescriptorType = new QLabel(QString("%1/%2").arg(obj->detectorType()).arg(obj->descriptorType()), this); + QLabel * title = new QLabel(QString("%1 (%2)").arg(obj->id()).arg(obj->keypoints().size()), this); + QLabel * detectorDescriptorType = new QLabel(QString("%1/%2").arg("detector").arg("descriptor"), this); QLabel * detectedLabel = new QLabel(this); title->setObjectName(QString("%1title").arg(obj->id())); detectorDescriptorType->setObjectName(QString("%1type").arg(obj->id())); @@ -692,7 +690,7 @@ void MainWindow::showObject(ObjWidget * obj) hLayout->addWidget(detectedLabel); vLayout->addLayout(hLayout); vLayout->addWidget(obj); - objects_.last()->setDeletable(true); + obj->setDeletable(true); connect(obj, SIGNAL(removalTriggered(ObjWidget*)), this, SLOT(removeObject(ObjWidget*))); connect(obj, SIGNAL(destroyed(QObject *)), title, SLOT(deleteLater())); connect(obj, SIGNAL(destroyed(QObject *)), detectedLabel, SLOT(deleteLater())); @@ -710,292 +708,54 @@ void MainWindow::showObject(ObjWidget * obj) } } -std::vector limitKeypoints(const std::vector & keypoints, int maxKeypoints) -{ - std::vector kptsKept; - if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints) - { - // Sort words by response - std::multimap reponseMap; // - for(unsigned int i = 0; i (fabs(keypoints[i].response), i)); - } - - // Remove them - std::multimap::reverse_iterator iter = reponseMap.rbegin(); - kptsKept.resize(maxKeypoints); - for(unsigned int k=0; k < kptsKept.size() && iter!=reponseMap.rend(); ++k, ++iter) - { - kptsKept[k] = keypoints[iter->second]; - } - } - else - { - kptsKept = keypoints; - } - return kptsKept; -} - -class ExtractFeaturesThread : public QThread -{ -public: - ExtractFeaturesThread(int objectId, int objectIndex, const cv::Mat & image) : - objectId_(objectId), - objectIndex_(objectIndex), - image_(image) - { - - } - int objectId() const {return objectId_;} - int objectIndex() const {return objectIndex_;} - const cv::Mat & image() const {return image_;} - const std::vector & keypoints() const {return keypoints_;} - const cv::Mat & descriptors() const {return descriptors_;} -protected: - virtual void run() - { - QTime time; - time.start(); - printf("Extracting descriptors from object %d...\n", objectId_); - KeypointDetector * detector = Settings::createKeypointDetector(); - keypoints_.clear(); - descriptors_ = cv::Mat(); - detector->detect(image_, keypoints_); - delete detector; - - if(keypoints_.size()) - { - int maxFeatures = Settings::getFeature2D_3MaxFeatures(); - if(maxFeatures > 0 && (int)keypoints_.size() > maxFeatures) - { - int previousCount = (int)keypoints_.size(); - keypoints_ = limitKeypoints(keypoints_, maxFeatures); - printf("obj=%d, %d keypoints removed, (kept %d), min/max response=%f/%f\n", objectId_, previousCount-(int)keypoints_.size(), (int)keypoints_.size(), keypoints_.size()?keypoints_.back().response:0.0f, keypoints_.size()?keypoints_.front().response:0.0f); - } - - DescriptorExtractor * extractor = Settings::createDescriptorExtractor(); - extractor->compute(image_, keypoints_, descriptors_); - delete extractor; - if((int)keypoints_.size() != descriptors_.rows) - { - printf("ERROR : obj=%d kpt=%d != descriptors=%d\n", objectId_, (int)keypoints_.size(), descriptors_.rows); - } - } - else - { - printf("WARNING: no features detected in object %d !?!\n", objectId_); - } - printf("%d descriptors extracted from object %d (in %d ms)\n", descriptors_.rows, objectId_, time.elapsed()); - } -private: - int objectId_; - int objectIndex_; - cv::Mat image_; - std::vector keypoints_; - cv::Mat descriptors_; -}; - void MainWindow::updateObjects() { - if(objects_.size()) + this->statusBar()->showMessage(tr("Updating %1 objects...").arg(findObject_->objects().size())); + QApplication::processEvents(); + + findObject_->updateObjects(); + + updateVocabulary(); + + QList signatures = findObject_->objects().values(); + for(int i=0; istatusBar()->showMessage(tr("Updating %1 objects...").arg(objects_.size())); - QApplication::processEvents(); + objWidgets_.value(signatures[i]->id())->setData(signatures[i]->keypoints(), cvtCvMat2QImage(signatures[i]->image())); - int threadCounts = Settings::getGeneral_threads(); - if(threadCounts == 0) - { - threadCounts = objects_.size(); - } - - QTime time; - time.start(); - printf("Features extraction from %d objects...\n", objects_.size()); - for(int i=0; i threads; - for(int k=i; kid(), k, objects_.at(k)->cvImage())); - threads.back()->start(); - } - - for(int j=0; jwait(); - - int index = threads[j]->objectIndex(); - - objects_.at(index)->setData(threads[j]->keypoints(), threads[j]->descriptors(), threads[j]->image(), Settings::currentDetectorType(), Settings::currentDescriptorType()); - - //update object labels - QLabel * title = qFindChild(this, QString("%1title").arg(objects_.at(index)->id())); - title->setText(QString("%1 (%2)").arg(objects_.at(index)->id()).arg(QString::number(objects_.at(index)->keypoints().size()))); - QLabel * detectorDescriptorType = qFindChild(this, QString("%1type").arg(objects_.at(index)->id())); - detectorDescriptorType->setText(QString("%1/%2").arg(objects_.at(index)->detectorType()).arg(objects_.at(index)->descriptorType())); - } - } - printf("Features extraction from %d objects... done! (%d ms)\n", objects_.size(), time.elapsed()); - - updateData(); + //update object labels + QLabel * title = qFindChild(this, QString("%1title").arg(signatures[i]->id())); + title->setText(QString("%1 (%2)").arg(signatures[i]->id()).arg(QString::number(signatures[i]->keypoints().size()))); + QLabel * detectorDescriptorType = qFindChild(this, QString("%1type").arg(signatures[i]->id())); + detectorDescriptorType->setText(QString("%1/%2").arg(signatures[i]->detectorType()).arg(signatures[i]->descriptorType())); } - if(!camera_->isRunning() && !ui_->imageView_source->cvImage().empty()) + + if(!camera_->isRunning() && !sceneImage_.empty()) { - this->update(ui_->imageView_source->cvImage()); + this->update(); } this->statusBar()->clearMessage(); } -void MainWindow::updateData() +void MainWindow::updateVocabulary() { - if(objects_.size()) - { - ui_->actionSave_objects->setEnabled(true); - } - else - { - ui_->actionSave_objects->setEnabled(false); - } + this->statusBar()->showMessage(tr("Updating vocabulary...")); + QApplication::processEvents(); - objectsDescriptors_.clear(); - dataRange_.clear(); - vocabulary_->clear(); - int count = 0; - int dim = -1; - int type = -1; - // Get the total size and verify descriptors - for(int i=0; idescriptors().empty()) - { - if(dim >= 0 && objects_.at(i)->descriptors().cols != dim) - { - if(this->isVisible()) - { - QMessageBox::critical(this, tr("Error"), tr("Descriptors of the objects are not all the same size!\nObjects opened must have all the same size (and from the same descriptor extractor).")); - } - else - { - printf("ERROR: Descriptors of the objects are not all the same size! Objects opened must have all the same size (and from the same descriptor extractor).\n"); - } - return; - } - dim = objects_.at(i)->descriptors().cols; - if(type >= 0 && objects_.at(i)->descriptors().type() != type) - { - if(this->isVisible()) - { - QMessageBox::critical(this, tr("Error"), tr("Descriptors of the objects are not all the same type!\nObjects opened must have been processed by the same descriptor extractor.")); - } - else - { - printf("ERROR: Descriptors of the objects are not all the same type! Objects opened must have been processed by the same descriptor extractor.\n"); - } - return; - } - type = objects_.at(i)->descriptors().type(); - count += objects_.at(i)->descriptors().rows; - } - } + QTime time; + findObject_->updateVocabulary(); - // Copy data - if(count) + if(findObject_->vocabulary()->size()) { - this->statusBar()->showMessage(tr("Updating objects data (%1 descriptors)...").arg(count)); - QApplication::processEvents(); - printf("Updating global descriptors matrix: Objects=%d, total descriptors=%d, dim=%d, type=%d\n", (int)objects_.size(), count, dim, type); - if(Settings::getGeneral_invertedSearch() || Settings::getGeneral_threads() == 1) - { - // If only one thread, put all descriptors in the same cv::Mat - objectsDescriptors_.push_back(cv::Mat(count, dim, type)); - int row = 0; - for(int i=0; idescriptors().rows) - { - cv::Mat dest(objectsDescriptors_[0], cv::Range(row, row+objects_.at(i)->descriptors().rows)); - objects_.at(i)->descriptors().copyTo(dest); - row += objects_.at(i)->descriptors().rows; - // dataRange contains the upper_bound for each - // object (the last descriptors position in the - // global object descriptors matrix) - if(objects_.at(i)->descriptors().rows) - { - dataRange_.insert(row-1, i); - } - } - } - - if(Settings::getGeneral_invertedSearch()) - { - QTime time; - time.start(); - bool incremental = Settings::getGeneral_vocabularyIncremental(); - if(incremental) - { - printf("Creating incremental vocabulary...\n"); - } - else - { - printf("Creating vocabulary...\n"); - } - QTime localTime; - localTime.start(); - int updateVocabularyMinWords = Settings::getGeneral_vocabularyUpdateMinWords(); - int addedWords = 0; - for(int i=0; i words = vocabulary_->addWords(objects_[i]->descriptors(), i, incremental); - objects_[i]->setWords(words); - addedWords += words.uniqueKeys().size(); - bool updated = false; - if(incremental && addedWords && addedWords >= updateVocabularyMinWords) - { - vocabulary_->update(); - addedWords = 0; - updated = true; - } - printf("Object %d, %d words from %d descriptors (%d words, %d ms) %s\n", - objects_[i]->id(), - words.uniqueKeys().size(), - objects_[i]->descriptors().rows, - vocabulary_->size(), - localTime.restart(), - updated?"updated":""); - } - if(addedWords) - { - vocabulary_->update(); - } - ui_->label_timeIndexing->setNum(time.elapsed()); - ui_->label_vocabularySize->setNum(vocabulary_->size()); - if(incremental) - { - printf("Creating incremental vocabulary... done! size=%d (%d ms)\n", vocabulary_->size(), time.elapsed()); - } - else - { - printf("Creating vocabulary... done! size=%d (%d ms)\n", vocabulary_->size(), time.elapsed()); - } - } - } - else - { - for(int i=0; idescriptors()); - } - } - this->statusBar()->clearMessage(); + ui_->label_timeIndexing->setNum(time.elapsed()); + ui_->label_vocabularySize->setNum(findObject_->vocabulary()->size()); } lastObjectsUpdateParameters_ = Settings::getParameters(); + this->statusBar()->clearMessage(); } void MainWindow::startProcessing() { - printf("Starting camera...\n"); + UINFO("Starting camera..."); bool updateStatusMessage = this->statusBar()->currentMessage().isEmpty(); if(updateStatusMessage) { @@ -1036,25 +796,14 @@ void MainWindow::startProcessing() { this->statusBar()->clearMessage(); } - if(this->isVisible()) + + if(Settings::getCamera_6useTcpCamera()) { - if(Settings::getCamera_6useTcpCamera()) - { - QMessageBox::critical(this, tr("Camera error"), tr("Camera initialization failed! (with server %1:%2)").arg(Settings::getCamera_7IP()).arg(Settings::getCamera_8port())); - } - else - { - QMessageBox::critical(this, tr("Camera error"), tr("Camera initialization failed! (with device %1)").arg(Settings::getCamera_1deviceId())); - } - } - else if(Settings::getCamera_6useTcpCamera()) - { - printf("%s\n", tr("Camera initialization failed! (with server %1:%2) Trying again in 1 second...").arg(Settings::getCamera_7IP()).arg(Settings::getCamera_8port()).toStdString().c_str()); - QTimer::singleShot(1000, this, SLOT(startProcessing())); + QMessageBox::critical(this, tr("Camera error"), tr("Camera initialization failed! (with server %1:%2)").arg(Settings::getCamera_7IP()).arg(Settings::getCamera_8port())); } else { - printf("[ERROR] Camera initialization failed! (with device %d)\n", Settings::getCamera_1deviceId()); + QMessageBox::critical(this, tr("Camera error"), tr("Camera initialization failed! (with device %1)").arg(Settings::getCamera_1deviceId())); } } } @@ -1124,672 +873,212 @@ void MainWindow::rectHovered(int objId) } } -class SearchThread: public QThread -{ -public: - SearchThread(Vocabulary * vocabulary, int objectIndex, const cv::Mat * descriptors, const ObjWidget * sceneObject) : - vocabulary_(vocabulary), - objectIndex_(objectIndex), - descriptors_(descriptors), - sceneObject_(sceneObject), - minMatchedDistance_(-1.0f), - maxMatchedDistance_(-1.0f) - { - Q_ASSERT(index && descriptors); - } - virtual ~SearchThread() {} - - int getObjectIndex() const {return objectIndex_;} - float getMinMatchedDistance() const {return minMatchedDistance_;} - float getMaxMatchedDistance() const {return maxMatchedDistance_;} - const QMultiMap & getMatches() const {return matches_;} - -protected: - virtual void run() - { - //QTime time; - //time.start(); - - cv::Mat results; - cv::Mat dists; - - //match objects to scene - int k = Settings::getNearestNeighbor_3nndrRatioUsed()?2:1; - results = cv::Mat(descriptors_->rows, k, CV_32SC1); // results index - dists = cv::Mat(descriptors_->rows, k, CV_32FC1); // Distance results are CV_32FC1 - vocabulary_->search(*descriptors_, results, dists, k); - - // PROCESS RESULTS - // Get all matches for each object - for(int i=0; i(i,0) <= Settings::getNearestNeighbor_4nndrRatio() * dists.at(i,1)) - { - matched = true; - } - if((matched || !Settings::getNearestNeighbor_3nndrRatioUsed()) && - Settings::getNearestNeighbor_5minDistanceUsed()) - { - if(dists.at(i,0) <= Settings::getNearestNeighbor_6minDistance()) - { - matched = true; - } - else - { - matched = false; - } - } - if(!matched && !Settings::getNearestNeighbor_3nndrRatioUsed() && !Settings::getNearestNeighbor_5minDistanceUsed()) - { - matched = true; // no criterion, match to the nearest descriptor - } - if(minMatchedDistance_ == -1 || minMatchedDistance_ > dists.at(i,0)) - { - minMatchedDistance_ = dists.at(i,0); - } - if(maxMatchedDistance_ == -1 || maxMatchedDistance_ < dists.at(i,0)) - { - maxMatchedDistance_ = dists.at(i,0); - } - - int wordId = results.at(i,0); - if(matched && sceneObject_->words().count(wordId) == 1) - { - matches_.insert(i, sceneObject_->words().value(wordId)); - matches_.insert(i, results.at(i,0)); - } - } - - //printf("Search Object %d time=%d ms\n", objectIndex_, time.elapsed()); - } -private: - Vocabulary * vocabulary_; // would be const but flann search() method is not const!? - int objectIndex_; - const cv::Mat * descriptors_; - const ObjWidget * sceneObject_; - - float minMatchedDistance_; - float maxMatchedDistance_; - QMultiMap matches_; -}; - -class HomographyThread: public QThread -{ -public: - HomographyThread( - const QMultiMap * matches, // - int objectIndex, - const std::vector * kptsA, - const std::vector * kptsB) : - matches_(matches), - objectIndex_(objectIndex), - kptsA_(kptsA), - kptsB_(kptsB), - inliers_(0), - outliers_(0) - { - Q_ASSERT(matches && kptsA && kptsB); - } - virtual ~HomographyThread() {} - - int getObjectIndex() const {return objectIndex_;} - const std::vector & getIndexesA() const {return indexesA_;} - const std::vector & getIndexesB() const {return indexesB_;} - const std::vector & getOutlierMask() const {return outlierMask_;} - int getInliers() const {return inliers_;} - int getOutliers() const {return outliers_;} - const cv::Mat & getHomography() const {return h_;} - -protected: - virtual void run() - { - //QTime time; - //time.start(); - - std::vector mpts_1(matches_->size()); - std::vector mpts_2(matches_->size()); - indexesA_.resize(matches_->size()); - indexesB_.resize(matches_->size()); - - int j=0; - for(QMultiMap::const_iterator iter = matches_->begin(); iter!=matches_->end(); ++iter) - { - mpts_1[j] = kptsA_->at(iter.key()).pt; - indexesA_[j] = iter.key(); - mpts_2[j] = kptsB_->at(iter.value()).pt; - indexesB_[j] = iter.value(); - ++j; - } - - if((int)mpts_1.size() >= Settings::getHomography_minimumInliers()) - { - h_ = findHomography(mpts_1, - mpts_2, - Settings::getHomographyMethod(), - Settings::getHomography_ransacReprojThr(), - outlierMask_); - - for(unsigned int k=0; k * matches_; - int objectIndex_; - const std::vector * kptsA_; - const std::vector * kptsB_; - - std::vector indexesA_; - std::vector indexesB_; - std::vector outlierMask_; - int inliers_; - int outliers_; - cv::Mat h_; -}; - void MainWindow::update(const cv::Mat & image) { - //printf("====== UPDATE =======\n"); - - QTime totalTime; - totalTime.start(); // reset objects color - for(int i=0; i::iterator iter=objWidgets_.begin(); iter!=objWidgets_.end(); ++iter) { - objects_[i]->resetKptsColor(); + iter.value()->resetKptsColor(); } - if(!image.empty()) { - //Convert to grayscale - cv::Mat grayscaleImg; - if(image.channels() != 1 || image.depth() != CV_8U) - { - cv::cvtColor(image, grayscaleImg, cv::COLOR_BGR2GRAY); - } - else - { - grayscaleImg = image; - } + sceneImage_ = image.clone(); + } - QTime time; - time.start(); + QMultiMap > objectsDetected; + if(findObject_->detect(sceneImage_, objectsDetected)) + { + ui_->label_timeDetection->setNum(findObject_->timeStamps().value(FindObject::kTimeKeypointDetection, 0)); + ui_->label_timeExtraction->setNum(findObject_->timeStamps().value(FindObject::kTimeDescriptorExtraction, 0)); + ui_->imageView_source->setData(findObject_->sceneKeypoints(), cvtCvMat2QImage(sceneImage_)); + ui_->label_timeIndexing->setNum(findObject_->timeStamps().value(FindObject::kTimeIndexing, 0)); + ui_->label_vocabularySize->setNum(findObject_->vocabulary()->size()); + ui_->label_timeMatching->setNum(findObject_->timeStamps().value(FindObject::kTimeMatching, 0)); + ui_->label_timeHomographies->setNum(findObject_->timeStamps().value(FindObject::kTimeHomography, 0)); - // EXTRACT KEYPOINTS - std::vector keypoints; - detector_->detect(grayscaleImg, keypoints); - ui_->label_timeDetection->setNum(time.restart()); - - cv::Mat descriptors; - if(keypoints.size()) + // Colorize features matched + const QMap > & matches = findObject_->matches(); + QMap scores; + int maxScoreId = -1; + int maxScore = 0; + for(QMap >::const_iterator jter=matches.constBegin(); jter!=matches.end();++jter) { - int maxFeatures = Settings::getFeature2D_3MaxFeatures(); - if(maxFeatures > 0 && (int)keypoints.size() > maxFeatures) + scores.insert(jter.key(), jter.value().size()); + if(maxScoreId == -1 || maxScore < jter.value().size()) { - //int previousCount = (int)keypoints.size(); - keypoints = limitKeypoints(keypoints, maxFeatures); - //printf("%d keypoints removed, (kept %d), min/max response=%f/%f\n", previousCount-(int)keypoints.size(), (int)keypoints.size(), keypoints.size()?keypoints.back().response:0.0f, keypoints.size()?keypoints.front().response:0.0f); + maxScoreId = jter.key(); + maxScore = jter.value().size(); } - // EXTRACT DESCRIPTORS - extractor_->compute(grayscaleImg, keypoints, descriptors); - if((int)keypoints.size() != descriptors.rows) + int id = jter.key(); + QLabel * label = ui_->dockWidget_objects->findChild(QString("%1detection").arg(id)); + if(!Settings::getHomography_homographyComputed()) { - printf("ERROR : kpt=%d != descriptors=%d\n", (int)keypoints.size(), descriptors.rows); - } - ui_->label_timeExtraction->setNum(time.restart()); - } - else - { - printf("WARNING: no features detected !?!\n"); - ui_->label_timeExtraction->setNum(0); - } + label->setText(QString("%1 matches").arg(jter.value().size())); - if(this->isVisible()) - { - ui_->imageView_source->setData(keypoints, cv::Mat(), image, Settings::currentDetectorType(), Settings::currentDescriptorType()); - } + ObjWidget * obj = objWidgets_.value(id); + Q_ASSERT(obj != 0); - bool consistentNNData = (vocabulary_->size()!=0 && vocabulary_->wordToObjects().begin().value()!=-1 && Settings::getGeneral_invertedSearch()) || - ((vocabulary_->size()==0 || vocabulary_->wordToObjects().begin().value()==-1) && !Settings::getGeneral_invertedSearch()); - - // COMPARE - if(!objectsDescriptors_.empty() && - keypoints.size() && - consistentNNData && - objectsDescriptors_[0].cols == descriptors.cols && - objectsDescriptors_[0].type() == descriptors.type()) // binary descriptor issue, if the dataTree is not yet updated with modified settings - { - QVector > matches(objects_.size()); // Map< ObjectDescriptorIndex, SceneDescriptorIndex > - QVector matchesId(objects_.size(), -1); - float minMatchedDistance = -1.0f; - float maxMatchedDistance = -1.0f; - - if(!Settings::getGeneral_invertedSearch()) - { - // CREATE INDEX for the scene - //printf("Creating FLANN index (%s)\n", Settings::currentNearestNeighborType().toStdString().c_str()); - vocabulary_->clear(); - QMultiMap words = vocabulary_->addWords(descriptors, -1, Settings::getGeneral_vocabularyIncremental()); - if(!Settings::getGeneral_vocabularyIncremental()) + int nColor = id % 11 + 7; + QColor color((Qt::GlobalColor)(nColor==Qt::yellow?Qt::gray:nColor)); + for(QMultiMap::const_iterator iter = jter.value().constBegin(); iter!= jter.value().constEnd(); ++iter) { - vocabulary_->update(); + obj->setKptColor(iter.key(), color); + ui_->imageView_source->setKptColor(iter.value(), color); } - ui_->imageView_source->setWords(words); - ui_->label_timeIndexing->setNum(time.restart()); - ui_->label_vocabularySize->setNum(vocabulary_->size()); } - - if(Settings::getGeneral_invertedSearch() || Settings::getGeneral_threads() == 1) + else if(!objectsDetected.contains(id)) { - cv::Mat results; - cv::Mat dists; - // DO NEAREST NEIGHBOR - int k = Settings::getNearestNeighbor_3nndrRatioUsed()?2:1; - if(!Settings::getGeneral_invertedSearch()) + // Homography could not be computed... + QLabel * label = ui_->dockWidget_objects->findChild(QString("%1detection").arg(id)); + QMultiMap rejectedInliers = findObject_->rejectedInliers().value(id); + QMultiMap rejectedOutliers = findObject_->rejectedOutliers().value(id); + if(jter.value().size() < Settings::getHomography_minimumInliers()) { - //match objects to scene - results = cv::Mat(objectsDescriptors_[0].rows, k, CV_32SC1); // results index - dists = cv::Mat(objectsDescriptors_[0].rows, k, CV_32FC1); // Distance results are CV_32FC1 - vocabulary_->search(objectsDescriptors_[0], results, dists, k); + label->setText(QString("Too low matches (%1)").arg(jter.value().size())); + } + else if(rejectedInliers.size() >= Settings::getHomography_minimumInliers()) + { + label->setText(QString("Ignored, all inliers (%1 in %2 out)").arg(rejectedInliers.size()).arg(rejectedOutliers.size())); } else { - //match scene to objects - results = cv::Mat(descriptors.rows, k, CV_32SC1); // results index - dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1 - vocabulary_->search(descriptors, results, dists, k); + label->setText(QString("Too low inliers (%1 in %2 out)").arg(rejectedInliers.size()).arg(rejectedOutliers.size())); } - // PROCESS RESULTS - // Get all matches for each object - for(int i=0; i(i,0) <= Settings::getNearestNeighbor_4nndrRatio() * dists.at(i,1)) - { - matched = true; - } - if((matched || !Settings::getNearestNeighbor_3nndrRatioUsed()) && - Settings::getNearestNeighbor_5minDistanceUsed()) - { - if(dists.at(i,0) <= Settings::getNearestNeighbor_6minDistance()) - { - matched = true; - } - else - { - matched = false; - } - } - if(!matched && !Settings::getNearestNeighbor_3nndrRatioUsed() && !Settings::getNearestNeighbor_5minDistanceUsed()) - { - matched = true; // no criterion, match to the nearest descriptor - } - if(minMatchedDistance == -1 || minMatchedDistance > dists.at(i,0)) - { - minMatchedDistance = dists.at(i,0); - } - if(maxMatchedDistance == -1 || maxMatchedDistance < dists.at(i,0)) - { - maxMatchedDistance = dists.at(i,0); - } + // Add homography rectangles when homographies are computed + QMultiMap >::const_iterator inliersIter = findObject_->inliers().constBegin(); + QMultiMap >::const_iterator outliersIter = findObject_->outliers().constBegin(); + for(QMultiMap >::iterator iter = objectsDetected.begin(); + iter!=objectsDetected.end() && inliersIter!=findObject_->inliers().constEnd(); + ++iter, ++inliersIter) + { + int id = iter.key(); + ObjWidget * obj = objWidgets_.value(id); + Q_ASSERT(obj != 0); - if(matched) - { - if(Settings::getGeneral_invertedSearch()) - { - int wordId = results.at(i,0); - QList objIndexes = vocabulary_->wordToObjects().values(wordId); - for(int j=0; jwordToObjects().count(wordId, objIndexes[j]) == 1) - { - matches[objIndexes[j]].insert(objects_.at(objIndexes[j])->words().value(wordId), i); - } - } - } - else - { - QMap::iterator iter = dataRange_.lowerBound(i); - int objectIndex = iter.value(); - int fisrtObjectDescriptorIndex = (iter == dataRange_.begin())?0:(--iter).key()+1; - int objectDescriptorIndex = i - fisrtObjectDescriptorIndex; + // COLORIZE (should be done in the GUI thread) + int nColor = id % 11 + 7; + QColor color((Qt::GlobalColor)(nColor==Qt::yellow?Qt::gray:nColor)); - int wordId = results.at(i,0); - if(ui_->imageView_source->words().count(wordId) == 1) - { - matches[objectIndex].insert(objectDescriptorIndex, ui_->imageView_source->words().value(wordId)); - } - } - } - } + QTransform hTransform = iter.value().second; + + QRect rect = obj->pixmap().rect(); + // add rectangle + QPen rectPen(color); + rectPen.setWidth(Settings::getHomography_rectBorderWidth()); + RectItem * rectItemScene = new RectItem(id, rect); + connect(rectItemScene, SIGNAL(hovered(int)), this, SLOT(rectHovered(int))); + rectItemScene->setPen(rectPen); + rectItemScene->setTransform(hTransform); + ui_->imageView_source->addRect(rectItemScene); + + QGraphicsRectItem * rectItemObj = new QGraphicsRectItem(rect); + rectItemObj->setPen(rectPen); + obj->addRect(rectItemObj); + + for(QMultiMap::const_iterator iter = inliersIter.value().constBegin(); iter!= inliersIter.value().constEnd(); ++iter) + { + obj->setKptColor(iter.key(), color); + ui_->imageView_source->setKptColor(iter.value(), color); + } + + QLabel * label = ui_->dockWidget_objects->findChild(QString("%1detection").arg(id)); + if(objectsDetected.count(id) > 1) + { + // if a homography is already found, set the objects count + label->setText(QString("%1 objects found").arg(objectsDetected.count(id))); } else { - //multi-threaded, match objects to scene - unsigned int threadCounts = Settings::getGeneral_threads(); - if(threadCounts == 0) - { - threadCounts = (int)objectsDescriptors_.size(); - } - for(unsigned int j=0; j threads; - - for(unsigned int k=j; kimageView_source)); - threads.back()->start(); - } - - for(int k=0; kwait(); - matches[threads[k]->getObjectIndex()] = threads[k]->getMatches(); - - if(minMatchedDistance == -1 || minMatchedDistance > threads[k]->getMinMatchedDistance()) - { - minMatchedDistance = threads[k]->getMinMatchedDistance(); - } - if(maxMatchedDistance == -1 || maxMatchedDistance < threads[k]->getMaxMatchedDistance()) - { - maxMatchedDistance = threads[k]->getMaxMatchedDistance(); - } - delete threads[k]; - } - - } + label->setText(QString("%1 in %2 out").arg(inliersIter.value().size()).arg(outliersIter.value().size())); } - - ui_->label_timeMatching->setNum(time.restart()); - - // GUI: Homographies and color - int maxScoreId = -1; - int maxScore = 0; - QMultiMap > objectsDetected; - - QMap inliersScores; - if(Settings::getHomography_homographyComputed()) - { - // HOMOGRAHPY - int threadCounts = Settings::getGeneral_threads(); - if(threadCounts == 0) - { - threadCounts = matches.size(); - } - - for(int i=0; i threads; - - for(int k=i; k=0 ? matchesId[k]:k; // the first matches ids correspond object index - threads.push_back(new HomographyThread(&matches[k], objectId, &objects_.at(objectId)->keypoints(), &keypoints)); - threads.back()->start(); - } - - for(int j=0; jwait(); - - int index = threads[j]->getObjectIndex(); - - // COLORIZE (should be done in the GUI thread) - int nColor = index % 10 + 7; - QColor color((Qt::GlobalColor)(nColor==Qt::yellow?Qt::darkYellow:nColor)); - QLabel * label = ui_->dockWidget_objects->findChild(QString("%1detection").arg(objects_.at(index)->id())); - if(!threads[j]->getHomography().empty()) - { - inliersScores.insert(objects_.at(index)->id(), (float)threads[j]->getInliers()); - if(threads[j]->getInliers() >= Settings::getHomography_minimumInliers()) - { - const cv::Mat & H = threads[j]->getHomography(); - QTransform hTransform( - H.at(0,0), H.at(1,0), H.at(2,0), - H.at(0,1), H.at(1,1), H.at(2,1), - H.at(0,2), H.at(1,2), H.at(2,2)); - - int distance = Settings::getGeneral_multiDetectionRadius(); // in pixels - if(Settings::getGeneral_multiDetection()) - { - // Remove inliers and recompute homography - QMultiMap newMatches; - for(unsigned int k=0; kgetOutlierMask().size();++k) - { - if(!threads[j]->getOutlierMask().at(k)) - { - newMatches.insert(threads[j]->getIndexesA().at(k), threads[j]->getIndexesB().at(k)); - } - } - matches.push_back(newMatches); - matchesId.push_back(index); - - // compute distance from previous added same objects... - QMultiMap >::iterator objIter = objectsDetected.find(objects_.at(index)->id()); - for(;objIter!=objectsDetected.end() && objIter.key() == objects_.at(index)->id(); ++objIter) - { - qreal dx = objIter.value().second.m31() - hTransform.m31(); - qreal dy = objIter.value().second.m32() - hTransform.m32(); - int d = (int)sqrt(dx*dx + dy*dy); - if(d < distance) - { - distance = d; - } - } - } - - if(distance >= Settings::getGeneral_multiDetectionRadius()) - { - if(this->isVisible()) - { - for(unsigned int k=0; kgetOutlierMask().size();++k) - { - if(threads[j]->getOutlierMask().at(k)) - { - objects_.at(index)->setKptColor(threads[j]->getIndexesA().at(k), color); - ui_->imageView_source->setKptColor(threads[j]->getIndexesB().at(k), color); - } - else if(!objectsDetected.contains(objects_.at(index)->id())) - { - objects_.at(index)->setKptColor(threads[j]->getIndexesA().at(k), Qt::black); - } - } - } - - QRect rect = objects_.at(index)->pixmap().rect(); - objectsDetected.insert(objects_.at(index)->id(), QPair(rect, hTransform)); - // Example getting the center of the object in the scene using the homography - //QPoint pos(rect.width()/2, rect.height()/2); - //hTransform.map(pos) - - // add rectangle - if(this->isVisible()) - { - if(objectsDetected.count(objects_.at(index)->id()) > 1) - { - // if a homography is already found, set the objects count - label->setText(QString("%1 objects found").arg(objectsDetected.count(objects_.at(index)->id()))); - } - else - { - label->setText(QString("%1 in %2 out").arg(threads[j]->getInliers()).arg(threads[j]->getOutliers())); - } - QPen rectPen(color); - rectPen.setWidth(Settings::getHomography_rectBorderWidth()); - RectItem * rectItemScene = new RectItem(objects_.at(index)->id(), rect); - connect(rectItemScene, SIGNAL(hovered(int)), this, SLOT(rectHovered(int))); - rectItemScene->setPen(rectPen); - rectItemScene->setTransform(hTransform); - ui_->imageView_source->addRect(rectItemScene); - - QGraphicsRectItem * rectItemObj = new QGraphicsRectItem(rect); - rectItemObj->setPen(rectPen); - objects_.at(index)->addRect(rectItemObj); - } - } - } - else if(this->isVisible() && objectsDetected.count(objects_.at(index)->id()) == 0) - { - label->setText(QString("Too low inliers (%1 in %2 out)").arg(threads[j]->getInliers()).arg(threads[j]->getOutliers())); - } - } - else if(this->isVisible() && objectsDetected.count(objects_.at(index)->id()) == 0) - { - inliersScores.insert(objects_.at(index)->id(), 0.0f); - if(threads[j]->getInliers() >= Settings::getHomography_minimumInliers()) - { - label->setText(QString("Ignored, all inliers (%1 in %2 out)").arg(matches[index].size()).arg(threads[j]->getOutliers())); - } - else - { - label->setText(QString("Too low matches (%1)").arg(matches[index].size())); - } - } - } - } - ui_->label_timeHomographies->setNum(time.restart()); - } - else - { - for(int i=0; i::iterator iter = matches[i].begin(); iter!=matches[i].end(); ++iter) - { - objects_[i]->setKptColor(iter.key(), color); - ui_->imageView_source->setKptColor(iter.value(), color); - } - QLabel * label = ui_->dockWidget_objects->findChild(QString("%1detection").arg(objects_.at(i)->id())); - label->setText(QString("%1 matches").arg(matches[i].size())); - inliersScores.insert(objects_.at(i)->id(), 0.0f); - } - } - - //scores - QMap scores; - for(int i=0; i= 0? matchesId.at(i): i; - if(!scores.contains(objects_.at(objectIndex)->id())) - { - scores.insert(objects_.at(objectIndex)->id(), matches[i].size()); - } - // If objects detected, set max score to one detected with the most - // matches. Otherwise select any objects with the most matches. - if(objectsDetected.empty() || objectsDetected.contains(objects_.at(objectIndex)->id())) - { - if(maxScoreId == -1 || maxScore < matches[i].size()) - { - maxScoreId = objects_.at(objectIndex)->id(); - maxScore = matches[i].size(); - } - } - } - - //update likelihood plot - likelihoodCurve_->setData(scores, QMap()); - inliersCurve_->setData(inliersScores, QMap()); - if(ui_->likelihoodPlot->isVisible()) - { - ui_->likelihoodPlot->update(); - } - - ui_->label_minMatchedDistance->setNum(minMatchedDistance); - ui_->label_maxMatchedDistance->setNum(maxMatchedDistance); - - //Scroll objects slider to the best score - if(maxScoreId>=0 && Settings::getGeneral_autoScroll()) - { - QLabel * label = ui_->dockWidget_objects->findChild(QString("%1title").arg(maxScoreId)); - if(label) - { - ui_->objects_area->verticalScrollBar()->setValue(label->pos().y()); - } - } - - // Emit homographies - if(objectsDetected.size() > 1) - { - printf("(%s) %d objects detected!\n", - QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), - (int)objectsDetected.size()); - } - else if(objectsDetected.size() == 1) - { - printf("(%s) Object %d detected!\n", - QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), - (int)objectsDetected.begin().key()); - } - else if(Settings::getGeneral_sendNoObjDetectedEvents()) - { - printf("(%s) No objects detected.\n", - QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str()); - } - - if(objectsDetected.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents()) - { - Q_EMIT objectsFound(objectsDetected); - } - ui_->label_objectsDetected->setNum(objectsDetected.size()); } - else + + + //update likelihood plot + likelihoodCurve_->setData(scores, QMap()); + QMap inlierScores; + for(QMap::iterator iter=scores.begin(); iter!=scores.end(); ++iter) { - if(!objectsDescriptors_.empty() && keypoints.size()) + QList > values = findObject_->inliers().values(iter.key()); + int maxValue = 0; + if(values.size()) { - this->statusBar()->showMessage(tr("Cannot search, objects must be updated!")); - printf("Cannot search, objects must be updated!\n"); + maxValue = values[0].size(); + for(int i=1; iisVisible()) - { - ui_->imageView_source->setData(keypoints, cv::Mat(), image, Settings::currentDetectorType(), Settings::currentDescriptorType()); - } - + inlierScores.insert(iter.key(), maxValue); } - - if(this->isVisible()) + inliersCurve_->setData(inlierScores, QMap()); + if(ui_->likelihoodPlot->isVisible()) { - //Update object pictures - for(int i=0; ilikelihoodPlot->update(); + } + + ui_->label_minMatchedDistance->setNum(findObject_->minMatchedDistance()); + ui_->label_maxMatchedDistance->setNum(findObject_->maxMatchedDistance()); + + //Scroll objects slider to the best score + if(maxScoreId>=0 && Settings::getGeneral_autoScroll()) + { + QLabel * label = ui_->dockWidget_objects->findChild(QString("%1title").arg(maxScoreId)); + if(label) { - objects_[i]->update(); + ui_->objects_area->verticalScrollBar()->setValue(label->pos().y()); } } - ui_->label_nfeatures->setNum((int)keypoints.size()); - ui_->imageView_source->update(); + // Emit homographies + if(objectsDetected.size() > 1) + { + UINFO("(%s) %d objects detected!", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), + (int)objectsDetected.size()); + } + else if(objectsDetected.size() == 1) + { + UINFO("(%s) Object %d detected!", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str(), + (int)objectsDetected.begin().key()); + } + else if(Settings::getGeneral_sendNoObjDetectedEvents()) + { + UINFO("(%s) No objects detected.", + QTime::currentTime().toString("HH:mm:ss.zzz").toStdString().c_str()); + } + + if(objectsDetected.size() > 0 || Settings::getGeneral_sendNoObjDetectedEvents()) + { + Q_EMIT objectsFound(objectsDetected); + } + ui_->label_objectsDetected->setNum(objectsDetected.size()); } + else + { + this->statusBar()->showMessage(tr("Cannot search, objects must be updated!")); + ui_->imageView_source->setData(findObject_->sceneKeypoints(), cvtCvMat2QImage(sceneImage_)); + } + + + //Update object pictures + for(QMap::iterator iter=objWidgets_.begin(); iter!=objWidgets_.end(); ++iter) + { + iter.value()->update(); + } + + ui_->label_nfeatures->setNum((int)findObject_->sceneKeypoints().size()); + ui_->imageView_source->update(); + ui_->label_detectorDescriptorType->setText(QString("%1/%2").arg(Settings::currentDetectorType()).arg(Settings::currentDescriptorType())); //update slider @@ -1801,7 +1090,7 @@ void MainWindow::update(const cv::Mat & image) ui_->horizontalSlider_frames->blockSignals(false); } - ui_->label_timeTotal->setNum(totalTime.elapsed()); + ui_->label_timeTotal->setNum(findObject_->timeStamps().value(FindObject::kTimeTotal, 0)); int refreshRate = qRound(1000.0f/float(updateRate_.restart())); if(refreshRate > 0 && refreshRate < lowestRefreshRate_) { @@ -1830,10 +1119,10 @@ void MainWindow::notifyParametersChanged(const QStringList & paramChanged) bool nearestNeighborParamsChanged = false; for(QStringList::const_iterator iter = paramChanged.begin(); iter!=paramChanged.end(); ++iter) { - printf("Parameter changed: %s -> \"%s\"\n", iter->toStdString().c_str(), Settings::getParameter(*iter).toString().toStdString().c_str()); - printf("lastObjectsUpdateParameters_.value(*iter)=%s, Settings::getParameter(*iter)=%s", - lastObjectsUpdateParameters_.value(*iter).toString().toStdString().c_str(), - Settings::getParameter(*iter).toString().toStdString().c_str()); + UINFO("Parameter changed: %s -> \"%s\"", iter->toStdString().c_str(), Settings::getParameter(*iter).toString().toStdString().c_str()); + //UINFO("lastObjectsUpdateParameters_.value(*iter)=%s, Settings::getParameter(*iter)=%s", + // lastObjectsUpdateParameters_.value(*iter).toString().toStdString().c_str(), + // Settings::getParameter(*iter).toString().toStdString().c_str()); if(lastObjectsUpdateParameters_.value(*iter) != Settings::getParameter(*iter)) { if(!detectorDescriptorParamsChanged && iter->contains("Feature2D")) @@ -1862,11 +1151,7 @@ void MainWindow::notifyParametersChanged(const QStringList & paramChanged) if(detectorDescriptorParamsChanged) { //Re-init detector and extractor - delete detector_; - delete extractor_; - detector_ = Settings::createKeypointDetector(); - extractor_ = Settings::createDescriptorExtractor(); - Q_ASSERT(detector_ != 0 && extractor_ != 0); + findObject_->updateDetectorExtractor(); } if(Settings::getGeneral_autoUpdateObjects()) @@ -1877,16 +1162,16 @@ void MainWindow::notifyParametersChanged(const QStringList & paramChanged) } else if(nearestNeighborParamsChanged) { - this->updateData(); + this->updateVocabulary(); } } - else if(objects_.size() && (detectorDescriptorParamsChanged || nearestNeighborParamsChanged)) + else if(objWidgets_.size() && (detectorDescriptorParamsChanged || nearestNeighborParamsChanged)) { this->statusBar()->showMessage(tr("A parameter has changed... \"Update objects\" may be required.")); } - if(!camera_->isRunning() && !ui_->imageView_source->cvImage().empty()) + if(!camera_->isRunning() && !sceneImage_.empty()) { - this->update(ui_->imageView_source->cvImage()); + this->update(); ui_->label_timeRefreshRate->setVisible(false); } diff --git a/src/MainWindow.h b/src/MainWindow.h index b4b65826..16ad51ff 100644 --- a/src/MainWindow.h +++ b/src/MainWindow.h @@ -24,6 +24,7 @@ class TcpServer; class KeypointDetector; class DescriptorExtractor; class Vocabulary; +class FindObject; namespace rtabmap { @@ -35,15 +36,9 @@ class MainWindow : public QMainWindow Q_OBJECT public: - MainWindow(Camera * camera = 0, const QString & settings = "", QWidget * parent = 0); + MainWindow(FindObject * findObject, Camera * camera = 0, QWidget * parent = 0); virtual ~MainWindow(); - bool loadSettings(const QString & path); - bool saveSettings(const QString & path); - - int loadObjects(const QString & dirPath); - void saveObjects(const QString & dirPath); - void setSourceImageText(const QString & text); protected: @@ -70,7 +65,7 @@ private Q_SLOTS: void updateObjectsSize(); void updateMirrorView(); void showHideControls(); - void update(const cv::Mat & image); + void update(const cv::Mat & image = cv::Mat()); void updateObjects(); void notifyParametersChanged(const QStringList & param); void moveCameraFrame(int frame); @@ -80,32 +75,32 @@ Q_SIGNALS: void objectsFound(const QMultiMap > &); private: + bool loadSettings(const QString & path); + bool saveSettings(const QString & path); + int loadObjects(const QString & dirPath); + int saveObjects(const QString & dirPath); void setupTCPServer(); - void addObjectFromFile(const QString & filePath); + bool addObjectFromFile(const QString & filePath); void showObject(ObjWidget * obj); - void updateData(); void updateObjectSize(ObjWidget * obj); + void updateVocabulary(); private: Ui_mainWindow * ui_; Camera * camera_; - QString settings_; + FindObject * findObject_; rtabmap::PdfPlotCurve * likelihoodCurve_; rtabmap::PdfPlotCurve * inliersCurve_; AboutDialog * aboutDialog_; - QList objects_; - std::vector objectsDescriptors_; - Vocabulary * vocabulary_; - QMap dataRange_; // + QMap objWidgets_; QTime updateRate_; QTime refreshStartTime_; int lowestRefreshRate_; bool objectsModified_; QMap imagesMap_; - TcpServer * tcpServer_; QMap lastObjectsUpdateParameters_; // ParametersMap - KeypointDetector * detector_; - DescriptorExtractor * extractor_; + TcpServer * tcpServer_; + cv::Mat sceneImage_; }; #endif /* MainWindow_H_ */ diff --git a/src/ObjSignature.h b/src/ObjSignature.h new file mode 100644 index 00000000..b9ab6bb3 --- /dev/null +++ b/src/ObjSignature.h @@ -0,0 +1,57 @@ +/* + * ObjSignature.h + * + * Created on: 2014-07-30 + * Author: mathieu + */ + +#ifndef OBJSIGNATURE_H_ +#define OBJSIGNATURE_H_ + +#include +#include +#include +#include + +class ObjSignature { +public: + ObjSignature(int id, const cv::Mat & image) : + id_(id), + image_(image) + {} + virtual ~ObjSignature() {} + + void setData(const std::vector & keypoints, + const cv::Mat & descriptors, + const QString & detectorType, + const QString & descriptorType) + { + keypoints_ = keypoints; + descriptors_ = descriptors; + detectorType_ = detectorType; + descriptorType_ = descriptorType; + } + void setWords(const QMultiMap & words) {words_ = words;} + void setId(int id) {id_ = id;} + + QRect rect() const {return QRect(0,0,image_.cols, image_.rows);} + + int id() const {return id_;} + const cv::Mat & image() const {return image_;} + const std::vector & keypoints() const {return keypoints_;} + const cv::Mat & descriptors() const {return descriptors_;} + const QMultiMap & words() const {return words_;} + const QString & detectorType() const {return detectorType_;} + const QString & descriptorType() const {return descriptorType_;} + +private: + int id_; + cv::Mat image_; + std::vector keypoints_; + cv::Mat descriptors_; + QMultiMap words_; // + QString detectorType_; + QString descriptorType_; +}; + +#endif /* OBJSIGNATURE_H_ */ diff --git a/src/ObjWidget.cpp b/src/ObjWidget.cpp index be2d1bb1..1fbdadf9 100644 --- a/src/ObjWidget.cpp +++ b/src/ObjWidget.cpp @@ -6,6 +6,7 @@ #include "KeypointItem.h" #include "QtOpenCV.h" #include "Settings.h" +#include "utilite/ULogger.h" #include @@ -29,32 +30,22 @@ ObjWidget::ObjWidget(QWidget * parent) : QWidget(parent), - graphicsView_(0), id_(0), - detectorType_("NA"), - descriptorType_("NA"), + graphicsView_(0), graphicsViewInitialized_(false), alpha_(100) { setupUi(); } -ObjWidget::ObjWidget(int id, - const std::vector & keypoints, - const cv::Mat & descriptors, - const cv::Mat & image, - const QString & detectorType, - const QString & descriptorType, - QWidget * parent) : +ObjWidget::ObjWidget(int id, const std::vector & keypoints, const QImage & image, QWidget * parent) : QWidget(parent), - graphicsView_(0), id_(id), - detectorType_("NA"), - descriptorType_("NA"), + graphicsView_(0), graphicsViewInitialized_(false), alpha_(100) { setupUi(); - this->setData(keypoints, descriptors, image, detectorType, descriptorType); + this->setData(keypoints, image); } ObjWidget::~ObjWidget() { @@ -240,38 +231,24 @@ void ObjWidget::setTextLabel(const QString & text) label_->setText(text); } -void ObjWidget::setData(const std::vector & keypoints, - const cv::Mat & descriptors, - const cv::Mat & image, - const QString & detectorType, - const QString & descriptorType) +void ObjWidget::setData(const std::vector & keypoints, const QImage & image) { keypoints_ = keypoints; - descriptors_ = descriptors; kptColors_ = QVector((int)keypoints.size(), defaultColor()); keypointItems_.clear(); rectItems_.clear(); graphicsView_->scene()->clear(); graphicsViewInitialized_ = false; - detectorType_ = detectorType; - descriptorType_ = descriptorType; mouseCurrentPos_ = mousePressedPos_; // this will reset roi selection - cvImage_ = image.clone(); - pixmap_ = QPixmap::fromImage(cvtCvMat2QImage(cvImage_)); + pixmap_ = QPixmap::fromImage(image); //this->setMinimumSize(image_.size()); if(graphicsViewMode_->isChecked()) { this->setupGraphicsView(); } - label_->setVisible(image.empty()); -} - -void ObjWidget::setWords(const QMultiMap & words) -{ - Q_ASSERT(words.size() == keypoints_.size()); - words_ = words; + label_->setVisible(image.isNull()); } void ObjWidget::resetKptsColor() @@ -296,7 +273,7 @@ void ObjWidget::setKptColor(int index, const QColor & color) } else { - printf("PROBLEM index =%d > size=%d\n", index, kptColors_.size()); + UWARN("PROBLEM index =%d > size=%d\n", index, kptColors_.size()); } if(graphicsViewMode_->isChecked()) @@ -381,64 +358,6 @@ void ObjWidget::setFeaturesShown(bool shown) } } -void ObjWidget::save(QDataStream & streamPtr) const -{ - streamPtr << id_ << detectorType_ << descriptorType_; - streamPtr << (int)keypoints_.size(); - for(unsigned int j=0; j kpts; - cv::Mat descriptors; - - int nKpts; - QString detectorType, descriptorType; - streamPtr >> id_ >> detectorType >> descriptorType >> nKpts; - for(int i=0;i> - kpt.angle >> - kpt.class_id >> - kpt.octave >> - kpt.pt.x >> - kpt.pt.y >> - kpt.response >> - kpt.size; - kpts.push_back(kpt); - } - - int rows,cols,type; - qint64 dataSize; - streamPtr >> rows >> cols >> type >> dataSize; - QByteArray data; - streamPtr >> data; - descriptors = cv::Mat(rows, cols, type, data.data()).clone(); - streamPtr >> pixmap_; - this->setData(kpts, descriptors, cv::Mat(), detectorType, descriptorType); - cvImage_ = cvtQImage2CvMat(pixmap_.toImage()); - //this->setMinimumSize(image_.size()); -} - void ObjWidget::computeScaleOffsets(float & scale, float & offsetX, float & offsetY) { scale = 1.0f; @@ -598,7 +517,7 @@ void ObjWidget::mouseReleaseEvent(QMouseEvent * event) right = qAbs(l - pixmap_.width()); } - Q_EMIT roiChanged(QRect(left, top, right-left, bottom-top)); + Q_EMIT roiChanged(cv::Rect(left, top, right-left, bottom-top)); } QWidget::mouseReleaseEvent(event); } diff --git a/src/ObjWidget.h b/src/ObjWidget.h index 2b4734df..c77d1674 100644 --- a/src/ObjWidget.h +++ b/src/ObjWidget.h @@ -25,22 +25,11 @@ class ObjWidget : public QWidget public: ObjWidget(QWidget * parent = 0); - ObjWidget(int id, - const std::vector & keypoints, - const cv::Mat & descriptors, - const cv::Mat & image, - const QString & detectorType = "NA", - const QString & descriptorType = "NA", - QWidget * parent = 0); + ObjWidget(int id, const std::vector & keypoints, const QImage & image, QWidget * parent = 0); virtual ~ObjWidget(); void setId(int id); - void setData(const std::vector & keypoints, - const cv::Mat & descriptors, - const cv::Mat & image, - const QString & detectorType, - const QString & descriptorType); - void setWords(const QMultiMap & words); + void setData(const std::vector & keypoints, const QImage & image); void setTextLabel(const QString & text); void resetKptsColor(); void setKptColor(int index, const QColor & color); @@ -55,12 +44,9 @@ public: void addRect(QGraphicsRectItem * rect); void clearRoiSelection() {mousePressedPos_ = mouseCurrentPos_ = QPoint();update();} - const QMultiMap & words() const {return words_;} - const std::vector & keypoints() const {return keypoints_;} - const cv::Mat & descriptors() const {return descriptors_;} - const QPixmap & pixmap() const {return pixmap_;} - const cv::Mat & cvImage() const {return cvImage_;} int id() const {return id_;} + const std::vector keypoints() const {return keypoints_;} + const QPixmap & pixmap() const {return pixmap_;} QColor defaultColor() const; bool isImageShown() const; bool isFeaturesShown() const; @@ -68,15 +54,10 @@ public: bool isMirrorView() const; //QGraphicsScene * scene() const; std::vector selectedKeypoints() const; - const QString & detectorType() const {return detectorType_;} - const QString & descriptorType() const {return descriptorType_;} QList selectedItems() const; QPixmap getSceneAsPixmap(); - void save(QDataStream & streamPtr) const; - void load(QDataStream & streamPtr); - protected: virtual void paintEvent(QPaintEvent *event); virtual void contextMenuEvent(QContextMenuEvent * event); @@ -88,7 +69,7 @@ protected: Q_SIGNALS: void removalTriggered(ObjWidget *); void selectionChanged(); - void roiChanged(const QRect &); + void roiChanged(const cv::Rect &); private: void setupGraphicsView(); @@ -98,17 +79,12 @@ private: void computeScaleOffsets(float & scale, float & offsetX, float & offsetY); private: + int id_; std::vector keypoints_; - cv::Mat descriptors_; - QMultiMap words_; // QPixmap pixmap_; - cv::Mat cvImage_; QList keypointItems_; QGraphicsView * graphicsView_; - int id_; QVector kptColors_; - QString detectorType_; - QString descriptorType_; QList rectItems_; bool graphicsViewInitialized_; int alpha_; diff --git a/src/QtOpenCV.cpp b/src/QtOpenCV.cpp index dc309cbd..1f275b20 100644 --- a/src/QtOpenCV.cpp +++ b/src/QtOpenCV.cpp @@ -6,25 +6,46 @@ #include #include -QImage cvtCvMat2QImage(const cv::Mat & image) +QImage cvtCvMat2QImage(const cv::Mat & image, bool isBgr) { QImage qtemp; if(!image.empty() && image.depth() == CV_8U) { - const unsigned char * data = image.data; - qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32); - for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize()) + if(image.channels()==3) { - for(int x = 0; x < image.cols; ++x) + const unsigned char * data = image.data; + if(image.channels() == 3) { - QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x; - *p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]); + qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32); + for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize()) + { + for(int x = 0; x < image.cols; ++x) + { + QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x; + if(isBgr) + { + *p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]); + } + else + { + *p = qRgb(data[x * image.channels()], data[x * image.channels()+1], data[x * image.channels()+2]); + } + } + } } } - } - else if(!image.empty() && image.depth() != CV_8U) - { - printf("Wrong image format, must be 8_bits\n"); + else if(image.channels() == 1) + { + // mono grayscale + qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy(); + QVector my_table; + for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i)); + qtemp.setColorTable(my_table); + } + else + { + printf("Wrong image format, must have 1 or 3 channels\n"); + } } return qtemp; } diff --git a/src/QtOpenCV.h b/src/QtOpenCV.h index a50fd7ea..4dfee64b 100644 --- a/src/QtOpenCV.h +++ b/src/QtOpenCV.h @@ -9,7 +9,7 @@ #include // Convert OpenCV matrix to QImage -QImage cvtCvMat2QImage(const cv::Mat & image); +QImage cvtCvMat2QImage(const cv::Mat & image, bool isBgr = true); // Convert QImage to OpenCV matrix cv::Mat cvtQImage2CvMat(const QImage & image); diff --git a/src/Settings.cpp b/src/Settings.cpp index 1ccd5991..1ce2b61c 100644 --- a/src/Settings.cpp +++ b/src/Settings.cpp @@ -4,6 +4,7 @@ #include "Settings.h" #include "Camera.h" +#include "utilite/ULogger.h" #include #include #include @@ -20,6 +21,7 @@ ParametersMap Settings::parameters_; ParametersType Settings::parametersType_; DescriptionsMap Settings::descriptions_; Settings Settings::dummyInit_; +QString Settings::iniPath_; QString Settings::workingDirectory() { @@ -39,12 +41,31 @@ QString Settings::iniDefaultPath() #endif } -void Settings::loadSettings(const QString & fileName, QByteArray * windowGeometry, QByteArray * windowState) +QString Settings::iniPath() +{ + if(!iniPath_.isNull()) + { + return iniPath_; + } + return iniDefaultPath(); +} + +void Settings::init(const QString & fileName) +{ + iniPath_ = fileName; + if(fileName.isEmpty()) + { + iniPath_ = iniDefaultPath(); + } + loadSettings(iniPath_); +} + +void Settings::loadSettings(const QString & fileName) { QString path = fileName; if(fileName.isEmpty()) { - path = iniDefaultPath(); + path = iniPath(); } QSettings ini(path, QSettings::IniFormat); for(ParametersMap::const_iterator iter = defaultParameters_.begin(); iter!=defaultParameters_.end(); ++iter) @@ -62,38 +83,53 @@ void Settings::loadSettings(const QString & fileName, QByteArray * windowGeometr str = getParameter(key).toString(); str[0] = index.toAscii(); value = QVariant(str); - printf("Updated list of parameter \"%s\"\n", key.toStdString().c_str()); + UINFO("Updated list of parameter \"%s\"", key.toStdString().c_str()); } setParameter(key, value); } } - if(windowGeometry) + if(cv::gpu::getCudaEnabledDeviceCount() == 0) { - QVariant value = ini.value("windowGeometry", QVariant()); - if(value.isValid()) - { - *windowGeometry = value.toByteArray(); - } - } - if(windowState) - { - QVariant value = ini.value("windowState", QVariant()); - if(value.isValid()) - { - *windowState = value.toByteArray(); - } + Settings::setFeature2D_SURF_gpu(false); + Settings::setFeature2D_Fast_gpu(false); + Settings::setFeature2D_ORB_gpu(false); } - printf("Settings loaded from %s\n", path.toStdString().c_str()); + UINFO("Settings loaded from %s", path.toStdString().c_str()); } -void Settings::saveSettings(const QString & fileName, const QByteArray & windowGeometry, const QByteArray & windowState) +void Settings::loadWindowSettings(QByteArray & windowGeometry, QByteArray & windowState, const QString & fileName) { QString path = fileName; if(fileName.isEmpty()) { - path = iniDefaultPath(); + path = iniPath(); + } + + QSettings ini(path, QSettings::IniFormat); + + QVariant value = ini.value("windowGeometry", QVariant()); + if(value.isValid()) + { + windowGeometry = value.toByteArray(); + } + + value = ini.value("windowState", QVariant()); + if(value.isValid()) + { + windowState = value.toByteArray(); + } + + UINFO("Window settings loaded from %s", path.toStdString().c_str()); +} + +void Settings::saveSettings(const QString & fileName) +{ + QString path = fileName; + if(fileName.isEmpty()) + { + path = iniPath(); } QSettings ini(path, QSettings::IniFormat); for(ParametersMap::const_iterator iter = parameters_.begin(); iter!=parameters_.end(); ++iter) @@ -108,6 +144,17 @@ void Settings::saveSettings(const QString & fileName, const QByteArray & windowG ini.setValue(iter.key(), iter.value()); } } + UINFO("Settings saved to %s", path.toStdString().c_str()); +} + +void Settings::saveWindowSettings(const QByteArray & windowGeometry, const QByteArray & windowState, const QString & fileName) +{ + QString path = fileName; + if(fileName.isEmpty()) + { + path = iniPath(); + } + QSettings ini(path, QSettings::IniFormat); if(!windowGeometry.isEmpty()) { ini.setValue("windowGeometry", windowGeometry); @@ -116,7 +163,7 @@ void Settings::saveSettings(const QString & fileName, const QByteArray & windowG { ini.setValue("windowState", windowState); } - printf("Settings saved to %s\n", path.toStdString().c_str()); + UINFO("Window settings saved to %s", path.toStdString().c_str()); } class GPUFeature2D @@ -161,7 +208,8 @@ public: } catch(cv::Exception &e) { - printf("GPUSURF error: %s \n(If something about layer_rows, parameter nOctaves=%d of SURF is too high for the size of the image (%d,%d).)\n", + UERROR("GPUSURF error: %s \n(If something about layer_rows, parameter nOctaves=%d of SURF " + "is too high for the size of the image (%d,%d).)", e.msg.c_str(), surf_.nOctaves, image.cols, @@ -182,7 +230,8 @@ public: } catch(cv::Exception &e) { - printf("GPUSURF error: %s \n(If something about layer_rows, parameter nOctaves=%d of SURF is too high for the size of the image (%d,%d).)\n", + UERROR("GPUSURF error: %s \n(If something about layer_rows, parameter nOctaves=%d of SURF " + "is too high for the size of the image (%d,%d).)", e.msg.c_str(), surf_.nOctaves, image.cols, @@ -226,7 +275,7 @@ protected: std::vector& keypoints, cv::Mat& descriptors) { - printf("GPUFAST:computeDescriptors() Should not be used!\n"); + UERROR("GPUFAST:computeDescriptors() Should not be used!"); } private: @@ -269,7 +318,7 @@ protected: } catch(cv::Exception &e) { - printf("GPUORB error: %s \n(If something about matrix size, the image/object may be too small (%d,%d).)\n", + UERROR("GPUORB error: %s \n(If something about matrix size, the image/object may be too small (%d,%d).)", e.msg.c_str(), image.cols, image.rows); @@ -289,7 +338,7 @@ protected: } catch(cv::Exception &e) { - printf("GPUORB error: %s \n(If something about matrix size, the image/object may be too small (%d,%d).)\n", + UERROR("GPUORB error: %s \n(If something about matrix size, the image/object may be too small (%d,%d).)", e.msg.c_str(), image.cols, image.rows); @@ -759,7 +808,7 @@ cv::flann::IndexParams * Settings::createFlannIndexParams() } if(!params) { - printf("ERROR: NN strategy not found !? Using default KDTRee...\n"); + UERROR("NN strategy not found !? Using default KDTRee..."); params = new cv::flann::KDTreeIndexParams(); } return params ; diff --git a/src/Settings.h b/src/Settings.h index 143b264c..f229bbdd 100644 --- a/src/Settings.h +++ b/src/Settings.h @@ -206,9 +206,14 @@ public: static QString workingDirectory(); static QString iniDefaultPath(); static QString iniDefaultFileName() {return "config.ini";} + static QString iniPath(); - static void loadSettings(const QString & fileName = QString(), QByteArray * windowGeometry = 0, QByteArray * windowState = 0); - static void saveSettings(const QString & fileName = QString(), const QByteArray & windowGeometry = QByteArray(), const QByteArray & windowState = QByteArray()); + static void init(const QString & fileName = QString()); + + static void loadSettings(const QString & fileName = QString()); + static void loadWindowSettings(QByteArray & windowGeometry, QByteArray & windowState, const QString & fileName = QString()); + static void saveSettings(const QString & fileName = QString()); + static void saveWindowSettings(const QByteArray & windowGeometry, const QByteArray & windowState, const QString & fileName = QString()); static const ParametersMap & getDefaultParameters() {return defaultParameters_;} static const ParametersMap & getParameters() {return parameters_;} @@ -240,6 +245,7 @@ private: static ParametersType parametersType_; static DescriptionsMap descriptions_; static Settings dummyInit_; + static QString iniPath_; }; class KeypointDetector diff --git a/src/TcpServer.cpp b/src/TcpServer.cpp index 1fe79cf1..dcb8fb03 100644 --- a/src/TcpServer.cpp +++ b/src/TcpServer.cpp @@ -6,6 +6,7 @@ */ #include "TcpServer.h" +#include "utilite/ULogger.h" #include #include @@ -16,7 +17,7 @@ TcpServer::TcpServer(quint16 port, QObject * parent) : { if (!this->listen(QHostAddress::Any, port)) { - printf("ERROR: Unable to start the TCP server: %s\n", this->errorString().toStdString().c_str()); + UERROR("Unable to start the TCP server: %s", this->errorString().toStdString().c_str()); return; } diff --git a/src/TcpServer.h b/src/TcpServer.h index 06dc48fe..178628fc 100644 --- a/src/TcpServer.h +++ b/src/TcpServer.h @@ -22,10 +22,11 @@ public: QHostAddress getHostAddress() const; quint16 getPort() const; +public Q_SLOTS: + void publishObjects(const QMultiMap > & objects); private Q_SLOTS: void addClient(); - void publishObjects(const QMultiMap > & objects); }; #endif /* TCPSERVER_H_ */ diff --git a/src/Vocabulary.cpp b/src/Vocabulary.cpp index f6901988..4baf3a79 100644 --- a/src/Vocabulary.cpp +++ b/src/Vocabulary.cpp @@ -26,7 +26,7 @@ void Vocabulary::clear() notIndexedWordIds_.clear(); } -QMultiMap Vocabulary::addWords(const cv::Mat & descriptors, int objectIndex, bool incremental) +QMultiMap Vocabulary::addWords(const cv::Mat & descriptors, int objectId, bool incremental) { QMultiMap words; if (descriptors.empty()) @@ -141,7 +141,7 @@ QMultiMap Vocabulary::addWords(const cv::Mat & descriptors, int object if(match) { words.insert(fullResults.begin().value(), i); - wordToObjects_.insert(fullResults.begin().value(), objectIndex); + wordToObjects_.insert(fullResults.begin().value(), objectId); ++matches; } else @@ -150,7 +150,7 @@ QMultiMap Vocabulary::addWords(const cv::Mat & descriptors, int object notIndexedWordIds_.push_back(indexedDescriptors_.rows + notIndexedDescriptors_.rows); notIndexedDescriptors_.push_back(descriptors.row(i)); words.insert(notIndexedWordIds_.back(), i); - wordToObjects_.insert(notIndexedWordIds_.back(), objectIndex); + wordToObjects_.insert(notIndexedWordIds_.back(), objectId); } } } @@ -158,7 +158,7 @@ QMultiMap Vocabulary::addWords(const cv::Mat & descriptors, int object { for(int i = 0; i < descriptors.rows; ++i) { - wordToObjects_.insert(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i, objectIndex); + wordToObjects_.insert(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i, objectId); words.insert(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i, i); notIndexedWordIds_.push_back(indexedDescriptors_.rows + notIndexedDescriptors_.rows+i); } diff --git a/src/Vocabulary.h b/src/Vocabulary.h index a236a850..c5f4733e 100644 --- a/src/Vocabulary.h +++ b/src/Vocabulary.h @@ -18,7 +18,7 @@ public: virtual ~Vocabulary(); void clear(); - QMultiMap addWords(const cv::Mat & descriptors, int objectIndex, bool incremental); + QMultiMap addWords(const cv::Mat & descriptors, int objectId, bool incremental); void update(); void search(const cv::Mat & descriptors, cv::Mat & results, cv::Mat & dists, int k); int size() const {return indexedDescriptors_.rows + notIndexedDescriptors_.rows;} @@ -28,7 +28,7 @@ private: cv::flann::Index flannIndex_; cv::Mat indexedDescriptors_; cv::Mat notIndexedDescriptors_; - QMultiMap wordToObjects_; + QMultiMap wordToObjects_; // QVector notIndexedWordIds_; }; diff --git a/src/rtabmap/PdfPlot.cpp b/src/rtabmap/PdfPlot.cpp index 7b01ad28..fba1a7a8 100644 --- a/src/rtabmap/PdfPlot.cpp +++ b/src/rtabmap/PdfPlot.cpp @@ -119,7 +119,7 @@ void PdfPlotCurve::clear() UPlotCurve::clear(); } -void PdfPlotCurve::setData(const QMap & dataMap, const QMap & weightsMap) +void PdfPlotCurve::setData(const QMap & dataMap, const QMap & weightsMap) { ULOGGER_DEBUG("dataMap=%d, weightsMap=%d", dataMap.size(), weightsMap.size()); if(dataMap.size() > 0) @@ -146,7 +146,7 @@ void PdfPlotCurve::setData(const QMap & dataMap, const QMap::iterator iter = _items.begin(); QMap::const_iterator j=weightsMap.begin(); - for(QMap::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i, ++j) + for(QMap::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i, ++j) { ((PdfPlotItem*)*iter)->setLikelihood(i.key(), i.value(), j!=weightsMap.end()?j.value():-1); //2 times... diff --git a/src/rtabmap/PdfPlot.h b/src/rtabmap/PdfPlot.h index 323b66c5..0c62403a 100644 --- a/src/rtabmap/PdfPlot.h +++ b/src/rtabmap/PdfPlot.h @@ -58,7 +58,7 @@ public: virtual ~PdfPlotCurve(); virtual void clear(); - void setData(const QMap & dataMap, const QMap & weightsMap); + void setData(const QMap & dataMap, const QMap & weightsMap); private: const QMap * _imagesMapRef; diff --git a/src/utilite/UDestroyer.h b/src/utilite/UDestroyer.h new file mode 100644 index 00000000..de0ad8e3 --- /dev/null +++ b/src/utilite/UDestroyer.h @@ -0,0 +1,77 @@ +/* +* utilite is a cross-platform library with +* useful utilities for fast and small developing. +* Copyright (C) 2010 Mathieu Labbe +* +* utilite is free library: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* utilite is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see . +*/ + +#ifndef UDESTROYER_H +#define UDESTROYER_H + +//#include "utilite/UtiLiteExp.h" // DLL export/import defines + +/** + * This class is used to delete a dynamically created + * objects. It was mainly designed to remove dynamically created Singleton. + * Created on the stack of a Singleton, when the + * application is finished, his destructor make sure that the + * Singleton is deleted. + * + */ +template +class UDestroyer +{ +public: + /** + * The constructor. Set the doomed object (take ownership of the object). The object is deleted + * when this object is deleted. + */ + UDestroyer(T* doomed = 0) : doomed_(doomed) {} + + ~UDestroyer() + { + if(doomed_) + { + delete doomed_; + doomed_ = 0; + } + } + + /** + * Set the doomed object. If a doomed object is already set, the function returns false. + * @param doomed the doomed object + * @return false if an object is already set and the new object is not null, otherwise true + */ + bool setDoomed(T* doomed) + { + if(doomed_ && doomed) + { + return false; + } + doomed_ = doomed; + return true; + } + +private: + // Prevent users from making copies of a + // Destroyer to avoid double deletion: + UDestroyer(const UDestroyer&); + void operator=(const UDestroyer&); + +private: + T* doomed_; +}; + +#endif // UDESTROYER_H diff --git a/src/utilite/ULogger.cpp b/src/utilite/ULogger.cpp new file mode 100644 index 00000000..009377c9 --- /dev/null +++ b/src/utilite/ULogger.cpp @@ -0,0 +1,590 @@ +/* +* utilite is a cross-platform library with +* useful utilities for fast and small developing. +* Copyright (C) 2010 Mathieu Labbe +* +* utilite is free library: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* utilite is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see . +*/ + +#include "utilite/ULogger.h" +#include "utilite/UConversion.h" +#include "utilite/UFile.h" +#include "utilite/UStl.h" +#include +#include +#include + +#ifndef WIN32 +#include +#endif + +#ifdef WIN32 +#include +#define COLOR_NORMAL FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED +#define COLOR_RED FOREGROUND_RED | FOREGROUND_INTENSITY +#define COLOR_GREEN FOREGROUND_GREEN +#define COLOR_YELLOW FOREGROUND_GREEN | FOREGROUND_RED +#else +#define COLOR_NORMAL "\033[0m" +#define COLOR_RED "\033[31m" +#define COLOR_GREEN "\033[32m" +#define COLOR_YELLOW "\033[33m" +#endif + +bool ULogger::append_ = true; +bool ULogger::printTime_ = true; +bool ULogger::printLevel_ = true; +bool ULogger::printEndline_ = true; +bool ULogger::printColored_ = true; +bool ULogger::printWhere_ = true; +bool ULogger::printWhereFullPath_ = false; +bool ULogger::limitWhereLength_ = false; +bool ULogger::buffered_ = false; +bool ULogger::exitingState_ = false; +ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error) +ULogger::Level ULogger::exitLevel_ = kFatal; +ULogger::Level ULogger::eventLevel_ = kFatal; +const char * ULogger::levelName_[5] = {"DEBUG", " INFO", " WARN", "ERROR", "FATAL"}; +ULogger* ULogger::instance_ = 0; +UDestroyer ULogger::destroyer_; +ULogger::Type ULogger::type_ = ULogger::kTypeNoLog; // Default nothing +UMutex ULogger::loggerMutex_; +const std::string ULogger::kDefaultLogFileName = "./ULog.txt"; +std::string ULogger::logFileName_; +std::string ULogger::bufferedMsgs_; + +/** + * This class is used to write logs in the console. This class cannot + * be directly used, use ULogger::setType() to console type to print in + * console and use macro UDEBUG(), UINFO()... to print messages. + * @see ULogger + */ +class UConsoleLogger : public ULogger +{ +public : + virtual ~UConsoleLogger() {this->_flush();} + +protected: + /** + * Only the Logger can create inherited + * loggers according to the Abstract factory patterns. + */ + friend class ULogger; + + UConsoleLogger() {} + +private: + virtual void _write(const char* msg, va_list arg) + { + vprintf(msg, arg); + } + virtual void _writeStr(const char* msg) + { + printf("%s", msg); + } +}; + +/** + * This class is used to write logs in a file. This class cannot + * be directly used, use ULogger::setType() to file type to print in + * a file and use macro UDEBUG(), UINFO()... to print messages. + * @see ULogger + */ +class UFileLogger : public ULogger +{ +public: + virtual ~UFileLogger() + { + this->_flush(); + if(fout_) + { + fclose(fout_); + } + } + +protected: + /** + * Only the Logger can create inherited + * loggers according to the Abstract factory patterns. + */ + friend class ULogger; + + /** + * The UFileLogger constructor. + * @param fileName the file name + * @param append if true append logs in the file, + * ortherwise it overrides the file. + * + */ + UFileLogger(const std::string &fileName, bool append) + { + fileName_ = fileName; + + if(!append) { + std::ofstream fileToClear(fileName_.c_str(), std::ios::out); + fileToClear.clear(); + fileToClear.close(); + } + +#ifdef _MSC_VER + fopen_s(&fout_, fileName_.c_str(), "a"); +#else + fout_ = fopen(fileName_.c_str(), "a"); +#endif + + if(!fout_) { + printf("FileLogger : Cannot open file : %s\n", fileName_.c_str()); // TODO send Event instead, or return error code + return; + } + } + +private: + virtual void _write(const char* msg, va_list arg) + { + if(fout_) + { + vfprintf(fout_, msg, arg); + } + } + virtual void _writeStr(const char* msg) + { + if(fout_) + { + fprintf(fout_, "%s", msg); + } + } + +private: + std::string fileName_; ///< the file name + FILE* fout_; + std::string bufferedMsgs_; +}; + +void ULogger::setType(Type type, const std::string &fileName, bool append) +{ + ULogger::flush(); + loggerMutex_.lock(); + { + // instance not yet created + if(!instance_) + { + type_ = type; + logFileName_ = fileName; + append_ = append; + instance_ = createInstance(); + } + // type changed + else if(type_ != type || (type_ == kTypeFile && logFileName_.compare(fileName)!=0)) + { + destroyer_.setDoomed(0); + delete instance_; + instance_ = 0; + type_ = type; + logFileName_ = fileName; + append_ = append; + instance_ = createInstance(); + } + } + loggerMutex_.unlock(); +} + +void ULogger::reset() +{ + ULogger::setType(ULogger::kTypeNoLog); + append_ = true; + printTime_ = true; + printLevel_ = true; + printEndline_ = true; + printColored_ = true; + printWhere_ = true; + printWhereFullPath_ = false; + limitWhereLength_ = false; + level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error) + logFileName_ = ULogger::kDefaultLogFileName; +} + +void ULogger::setBuffered(bool buffered) +{ + if(!buffered) + { + ULogger::flush(); + } + buffered_ = buffered; +} + + +void ULogger::flush() +{ + loggerMutex_.lock(); + if(!instance_ || bufferedMsgs_.size()==0) + { + loggerMutex_.unlock(); + return; + } + + instance_->_flush(); + loggerMutex_.unlock(); +} + +void ULogger::_flush() +{ + ULogger::getInstance()->_writeStr(bufferedMsgs_.c_str()); + bufferedMsgs_.clear(); +} + +void ULogger::write(const char* msg, ...) +{ + loggerMutex_.lock(); + if(!instance_) + { + loggerMutex_.unlock(); + return; + } + + std::string endline = ""; + if(printEndline_) { + endline = "\r\n"; + } + + std::string time = ""; + if(printTime_) + { + getTime(time); + time.append(" - "); + } + + + if(printTime_) + { + if(buffered_) + { + bufferedMsgs_.append(time.c_str()); + } + else + { + ULogger::getInstance()->_writeStr(time.c_str()); + } + } + + va_list args; + va_start(args, msg); + if(buffered_) + { + bufferedMsgs_.append(uFormatv(msg, args)); + } + else + { + ULogger::getInstance()->_write(msg, args); + } + va_end(args); + if(printEndline_) + { + if(buffered_) + { + bufferedMsgs_.append(endline.c_str()); + } + else + { + ULogger::getInstance()->_writeStr(endline.c_str()); + } + } + loggerMutex_.unlock(); + +} + +void ULogger::write(ULogger::Level level, + const char * file, + int line, + const char * function, + const char* msg, + ...) +{ + if(exitingState_) + { + // Ignore messages after a fatal exit... + return; + } + loggerMutex_.lock(); + if(type_ == kTypeNoLog && level < kFatal) + { + loggerMutex_.unlock(); + return; + } + if(strlen(msg) == 0 && !printWhere_ && level < exitLevel_) + { + loggerMutex_.unlock(); + // No need to show an empty message if we don't print where. + return; + } + + if(level >= level_) + { +#ifdef WIN32 + int color = 0; +#else + const char* color = NULL; +#endif + switch(level) + { + case kDebug: + color = COLOR_GREEN; + break; + case kInfo: + color = COLOR_NORMAL; + break; + case kWarning: + color = COLOR_YELLOW; + break; + case kError: + case kFatal: + color = COLOR_RED; + break; + default: + break; + } + + std::string endline = ""; + if(printEndline_) { + endline = "\r\n"; + } + + std::string time = ""; + if(printTime_) + { + time.append("("); + getTime(time); + time.append(") "); + } + + std::string levelStr = ""; + if(printLevel_) + { + const int bufSize = 30; + char buf[bufSize] = {0}; + +#ifdef _MSC_VER + sprintf_s(buf, bufSize, "[%s]", levelName_[level]); +#else + snprintf(buf, bufSize, "[%s]", levelName_[level]); +#endif + levelStr = buf; + levelStr.append(" "); + } + + std::string whereStr = ""; + if(printWhere_) + { + whereStr.append(""); + //File + if(printWhereFullPath_) + { + whereStr.append(file); + } + else + { + std::string fileName = UFile::getName(file); + if(limitWhereLength_ && fileName.size() > 8) + { + fileName.erase(8); + fileName.append("~"); + } + whereStr.append(fileName); + } + + //Line + whereStr.append(":"); + std::string lineStr = uNumber2Str(line); + whereStr.append(lineStr); + + //Function + whereStr.append("::"); + std::string funcStr = function; + if(!printWhereFullPath_ && limitWhereLength_ && funcStr.size() > 8) + { + funcStr.erase(8); + funcStr.append("~"); + } + funcStr.append("()"); + whereStr.append(funcStr); + + whereStr.append(" "); + } + + va_list args; + + if(type_ != kTypeNoLog) + { + va_start(args, msg); +#ifdef WIN32 + HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE); +#endif + if(type_ == ULogger::kTypeConsole && printColored_) + { +#ifdef WIN32 + SetConsoleTextAttribute(H,color); +#else + if(buffered_) + { + bufferedMsgs_.append(color); + } + else + { + ULogger::getInstance()->_writeStr(color); + } +#endif + } + + if(buffered_) + { + bufferedMsgs_.append(levelStr.c_str()); + bufferedMsgs_.append(time.c_str()); + bufferedMsgs_.append(whereStr.c_str()); + bufferedMsgs_.append(uFormatv(msg, args)); + } + else + { + ULogger::getInstance()->_writeStr(levelStr.c_str()); + ULogger::getInstance()->_writeStr(time.c_str()); + ULogger::getInstance()->_writeStr(whereStr.c_str()); + ULogger::getInstance()->_write(msg, args); + } + if(type_ == ULogger::kTypeConsole && printColored_) + { +#ifdef WIN32 + SetConsoleTextAttribute(H,COLOR_NORMAL); +#else + if(buffered_) + { + bufferedMsgs_.append(COLOR_NORMAL); + } + else + { + ULogger::getInstance()->_writeStr(COLOR_NORMAL); + } +#endif + } + if(buffered_) + { + bufferedMsgs_.append(endline.c_str()); + } + else + { + ULogger::getInstance()->_writeStr(endline.c_str()); + } + va_end (args); + } + + if(level >= exitLevel_) + { + printf("\n*******\n%s message occurred! Application will now exit.\n", levelName_[level]); + if(type_ != kTypeConsole) + { + printf(" %s%s%s\n", levelStr.c_str(), time.c_str(), whereStr.c_str()); + va_start(args, msg); + vprintf(msg, args); + va_end(args); + } + printf("*******\n"); + destroyer_.setDoomed(0); + delete instance_; // If a FileLogger is used, this will close the file. + instance_ = 0; + //======================================================================== + // EXIT APPLICATION + exit(1); + //======================================================================== + } + } + loggerMutex_.unlock(); +} + +int ULogger::getTime(std::string &timeStr) +{ + if(!printTime_) { + return 0; + } + struct tm timeinfo; + const int bufSize = 30; + char buf[bufSize] = {0}; + +#if _MSC_VER + time_t rawtime; + time(&rawtime); + localtime_s (&timeinfo, &rawtime ); + int result = sprintf_s(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d", + timeinfo.tm_year+1900, + (timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1, + (timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday, + (timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour, + (timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min, + (timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec); +#elif WIN32 + time_t rawtime; + time(&rawtime); + timeinfo = *localtime (&rawtime); + int result = snprintf(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d", + timeinfo.tm_year+1900, + (timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1, + (timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday, + (timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour, + (timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min, + (timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec); + #else + struct timeval rawtime; + gettimeofday(&rawtime, NULL); + localtime_r (&rawtime.tv_sec, &timeinfo); + int result = snprintf(buf, bufSize, "%d-%s%d-%s%d %s%d:%s%d:%s%d.%s%d", + timeinfo.tm_year+1900, + (timeinfo.tm_mon+1) < 10 ? "0":"", timeinfo.tm_mon+1, + (timeinfo.tm_mday) < 10 ? "0":"", timeinfo.tm_mday, + (timeinfo.tm_hour) < 10 ? "0":"", timeinfo.tm_hour, + (timeinfo.tm_min) < 10 ? "0":"", timeinfo.tm_min, + (timeinfo.tm_sec) < 10 ? "0":"", timeinfo.tm_sec, + (rawtime.tv_usec/1000) < 10 ? "00":(rawtime.tv_usec/1000) < 100?"0":"", int(rawtime.tv_usec/1000)); +#endif + if(result) + { + timeStr.append(buf); + } + return result; +} + +ULogger* ULogger::getInstance() +{ + if(!instance_) + { + instance_ = createInstance(); + } + return instance_; +} + +ULogger* ULogger::createInstance() +{ + ULogger* instance = 0; + if(type_ == ULogger::kTypeConsole) + { + instance = new UConsoleLogger(); + } + else if(type_ == ULogger::kTypeFile) + { + instance = new UFileLogger(logFileName_, append_); + } + destroyer_.setDoomed(instance); + return instance; +} + +ULogger::~ULogger() +{ + instance_ = 0; + //printf("Logger is destroyed...\n\r"); +} diff --git a/src/utilite/ULogger.h b/src/utilite/ULogger.h new file mode 100644 index 00000000..ff362b43 --- /dev/null +++ b/src/utilite/ULogger.h @@ -0,0 +1,531 @@ +/* +* utilite is a cross-platform library with +* useful utilities for fast and small developing. +* Copyright (C) 2010 Mathieu Labbe +* +* utilite is free library: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* utilite is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see . +*/ + +#ifndef ULOGGER_H +#define ULOGGER_H + +//#include "utilite/UtiLiteExp.h" // DLL export/import defines + +#include "utilite/UMutex.h" +#include "utilite/UDestroyer.h" + +#include +#include +#include +#include + +#include + +/** + * \file ULogger.h + * \brief ULogger class and convenient macros + * + * This contains macros useful for logging a message anywhere in the + * application. Once the ULogger is set, use these macros like a printf to + * print debug messages. +*/ + +/* + * Convenient macros for logging... + */ +#define ULOGGER_LOG(level, ...) ULogger::write(level, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__) + +#define ULOGGER_DEBUG(...) ULOGGER_LOG(ULogger::kDebug, __VA_ARGS__) +#define ULOGGER_INFO(...) ULOGGER_LOG(ULogger::kInfo, __VA_ARGS__) +#define ULOGGER_WARN(...) ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__) +#define ULOGGER_ERROR(...) ULOGGER_LOG(ULogger::kError, __VA_ARGS__) +#define ULOGGER_FATAL(...) ULOGGER_LOG(ULogger::kFatal, __VA_ARGS__) + +#define UDEBUG(...) ULOGGER_DEBUG(__VA_ARGS__) +#define UINFO(...) ULOGGER_INFO(__VA_ARGS__) +#define UWARN(...) ULOGGER_WARN(__VA_ARGS__) +#define UERROR(...) ULOGGER_ERROR(__VA_ARGS__) +#define UFATAL(...) ULOGGER_FATAL(__VA_ARGS__) + +#define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition) +#define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str) + +/** + * \def UDEBUG(...) + * Print a debug level message in the logger. Format is the same as a printf: + * @code + * UDEBUG("This is a debug message with the number %d", 42); + * @endcode + */ +/** + * \def UINFO(...) + * Print a information level message in the logger. Format is the same as a printf: + * @code + * UINFO("This is a information message with the number %d", 42); + * @endcode + */ +/** + * \def UWARN(...) + * Print a warning level message in the logger. Format is the same as a printf: + * @code + * UWARN("This is a warning message with the number %d", 42); + * @endcode + */ +/** + * \def UERROR(...) + * Print an error level message in the logger. Format is the same as a printf: + * @code + * UERROR("This is an error message with the number %d", 42); + * @endcode + */ +/** + * \def UFATAL(...) + * Print a fatal error level message in the logger. The application will exit on + * fatal error. Format is the same as a printf: + * @code + * UFATAL("This is a fatal error message with the number %d", 42); + * @endcode + */ +/** + * \def UASSERT(condition, ...) + * Print a fatal error level message in the logger if condition is not met. The application will exit on + * fatal error. Format is the same as a printf: + * @code + * UASSERT(a!=42, "This is a fatal error message with the number %d", 42); + * @endcode + */ + + +/** + * This class is used to log messages with time on a console, in a file + * and/or with an event. At the start of the application, call + * ULogger::setType() with the type of the logger you want (see ULogger::Type, the type of the output + * can be changed at the run-time.). To use it, + * simply call the convenient macros UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() depending of + * the severity of the message. You can disable some messages by setting the logger + * level ULogger::setLevel() to severity you want, defined by ULogger::Level. A fatal message + * will make the application to exit, printing the message on console (whatever the logger type) and + * posting a ULogEvent (synchronously... see UEventsManager::post()) before exiting. + * + * The display of the logged messages can be modified: + * - If you don't want the level label, set ULogger::setPrintLevel() to false. + * - If you don't want the time label, set ULogger::setPrintTime() to false. + * - If you don't want the end of line added, set ULogger::setPrintEndline() to false. + * - If you don't the full path of the message, set ULogger::setPrintWhereFullPath() to false. + * - If you don't the path of the message, set ULogger::setPrintWhere() to false. + * + * When using a file logger (kTypeLogger), it can be useful in some + * application to buffer messages before writing them to hard drive (avoiding + * hard drive latencies). You can set ULogger::setBuffered() to true to do that. When the + * buffered messages will be written to file on appllciation exit (ULogger destructor) or when + * ULogger::flush() is called. + * + * If you want the application to exit on a lower severity level than kFatal, + * you can set ULogger::setExitLevel() to any ULogger::Type you want. + * + * Example: + * @code + * #include + * int main(int argc, char * argv[]) + * { + * // Set the logger type. The choices are kTypeConsole, + * // kTypeFile or kTypeNoLog (nothing is logged). + * ULogger::setType(ULogger::kTypeConsole); + * + * // Set the logger severity level (kDebug, kInfo, kWarning, kError, kFatal). + * // All log entries under the severity level are not logged. Here, + * // only debug messages are not logged. + * ULogger::setLevel(ULogger::kInfo); + * + * // Use a predefined Macro to easy logging. It can be + * // called anywhere in the application as the logger is + * // a Singleton. + * UDEBUG("This message won't be logged because the " + * "severity level of the logger is set to kInfo."); + * + * UINFO("This message is logged."); + * + * UWARN("A warning message..."); + * + * UERROR("An error message with code %d.", 42); + * + * return 0; + * } + * @endcode + * Output: + * @code + * [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() This message is logged. + * [ WARN] (2010-09-25 18:08:20) main.cpp:20::main() A warning message... + * [ERROR] (2010-09-25 18:08:20) main.cpp:22::main() An error message with code 42. + * @endcode + * + * Another useful form of the ULogger is to use it with the UTimer class. Here an example: + * @code + * #include + * #include + * ... + * UTimer timer; // automatically starts + * // do some works for part A + * UINFO("Time for part A = %f s", timer.ticks()); + * // do some works for part B + * UINFO("Time for part B = %f s", timer.ticks()); + * // do some works for part C + * UINFO("Time for part C = %f s", timer.ticks()); + * ... + * @endcode + * + * @see setType() + * @see setLevel() + * @see UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() + * + */ +class ULogger +{ + +public: + /** + * The default log file name. + */ + static const std::string kDefaultLogFileName; + + /** + * Loggers available: + * @code + * kTypeNoLog, kTypeConsole, kTypeFile + * @endcode + */ + enum Type{kTypeNoLog, kTypeConsole, kTypeFile}; + + /** + * Logger levels, from lowest severity to highest: + * @code + * kDebug, kInfo, kWarning, kError, kFatal + * @endcode + */ + enum Level{kDebug, kInfo, kWarning, kError, kFatal}; + + /** + * Set the type of the logger. When using kTypeFile, the parameter "fileName" would be + * changed (default is "./ULog.txt"), and optionally "append" if we want the + * logger to append messages to file or to overwrite the file. + * @param type the ULogger::Type of the logger. + * @param fileName file name used with a file logger type. + * @param append if true, the file isn't overwritten, otherwise it is. + * + * TODO : Can it be useful to have 2 or more types at the same time ? Print + * in console and file at the same time. + */ + static void setType(Type type, const std::string &fileName = kDefaultLogFileName, bool append = true); + static Type type() {return type_;} + + // Setters + /** + * Print time: default true. + * @param printTime true to print time, otherwise set to false. + */ + static void setPrintTime(bool printTime) {printTime_ = printTime;} + static bool isPrintTime() {return printTime_;} + + /** + * Print level: default true. + * @param printLevel true to print level, otherwise set to false. + */ + static void setPrintLevel(bool printLevel) {printLevel_ = printLevel;} + static bool isPrintLevel() {return printLevel_;} + + /** + * Print end of line: default true. + * @param printLevel true to print end of line, otherwise set to false. + */ + static void setPrintEndline(bool printEndline) {printEndline_ = printEndline;} + static bool isPrintEndLine() {return printEndline_;} + + /** + * Print text with color: default true. + * Dark green for Debug, white for Info, yellow for Warning, red for Error and Fatal. + * @param printColored true to print text with color, otherwise set to false. + */ + static void setPrintColored(bool printColored) {printColored_ = printColored;} + static bool isPrintColored() {return printColored_;} + + /** + * Print where is this message in source code: default true. + * @param printWhere true to print where, otherwise set to false. + */ + static void setPrintWhere(bool printWhere) {printWhere_ = printWhere;} + static bool isPrintWhere() {return printWhere_;} + + /** + * Print the full path: default true. ULogger::setPrintWhere() must be true to have path printed. + * @param printWhereFullPath true to print the full path, otherwise set to false. + */ + static void setPrintWhereFullPath(bool printWhereFullPath) {printWhereFullPath_ = printWhereFullPath;} + static bool isPrintWhereFullPath() {return printWhereFullPath_;} + + /** + * Set is the logger buffers messages, default false. When true, the messages are + * buffered until the application is closed or ULogger::flush() is called. + * @see ULogger::flush() + * @param buffered true to buffer messages, otherwise set to false. + */ + static void setBuffered(bool buffered); + static bool isBuffered() {return buffered_;} + + /** + * Set logger level: default kInfo. All messages over the severity set + * are printed, other are ignored. The severity is from the lowest to + * highest: + * - kDebug + * - kInfo + * - kWarning + * - kError + * - kFatal + * @param level the minimum level of the messages printed. + */ + static void setLevel(ULogger::Level level) {level_ = level;} + static ULogger::Level level() {return level_;} + + /** + * Make application to exit when a log with level is written (useful for debugging). The message is printed to + * console (whatever the logger type) and an ULogEvent is sent (synchronously... see UEventsManager::post()) before exiting. + * + * Note : A kFatal level will always exit whatever the level specified here. + */ + static void setExitLevel(ULogger::Level exitLevel) {exitLevel_ = exitLevel;} + static ULogger::Level exitLevel() {return exitLevel_;} + + /** + * An ULogEvent is sent on each message logged at the specified level. + * Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()). + * @see ULogEvent + * @see setExitLevel() + */ + static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;} + static ULogger::Level eventLevel() {return eventLevel_;} + + /** + * Reset to default parameters. + */ + static void reset(); + + /** + * Flush buffered messages. + * @see setBuffered() + */ + static void flush(); + + /** + * Write a message directly to logger without level handling. + * @param msg the message to write. + * @param ... the variable arguments + * @deprecated use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL() + */ + static void write(const char* msg, ...); + + /* + * Write a message to logger: use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL() instead. + * @param level the log level of this message + * @param file the file path + * @param line the line in the file + * @param function the function name in which the message is logged + * @param msg the message to write + * @param ... the variable arguments + */ + static void write(ULogger::Level level, + const char * file, + int line, + const char *function, + const char* msg, + ...); + + /** + * Get the time in the format "2008-7-13 12:23:44". + * @param timeStr string were the time will be copied. + * @return the number of characters written, or 0 if an error occurred. + */ + static int getTime(std::string &timeStr); + +protected: + /* + * This method is used to have a reference on the + * Logger. When no Logger exists, one is + * created. There is only one instance in the application. + * Must be protected by loggerMutex_. + * See the Singleton pattern for further explanation. + * + * @return the reference on the Logger + */ + static ULogger* getInstance(); + + /* + * Called only once in getInstance(). It can't be instanciated + * by the user. + * + * @see getInstance() + */ + ULogger() {} + + /* + * Only called by a Destroyer. + * @see Destroyer + */ + virtual ~ULogger(); + + /* + * Flush buffered messages + */ + void _flush(); + + /* + * A Destroyer is used to remove a dynamicaly created + * Singleton. It is friend here to have access to the + * destructor. + * + * @see Destroyer + */ + friend class UDestroyer; + + /* + * The log file name. + */ + static std::string logFileName_; + + /* + * Default true, it doesn't overwrite the file. + */ + static bool append_; + +private: + /* + * Create an instance according to type. See the Abstract factory + * pattern for further explanation. + * @see type_ + * @return the reference on the new logger + */ + static ULogger* createInstance(); + + /* + * Write a message on the output with the format : + * "A message". Inherited class + * must override this method to output the message. It + * does nothing by default. + * @param msg the message to write. + * @param arg the variable arguments + */ + virtual void _write(const char* msg, va_list arg) {} // Do nothing by default + virtual void _writeStr(const char* msg) {} // Do nothing by default + +private: + /* + * The Logger instance pointer. + */ + static ULogger* instance_; + + /* + * The Logger's destroyer + */ + static UDestroyer destroyer_; + + /* + * If the logger prints the time for each message. + * Default is true. + */ + static bool printTime_; + + /* + * If the logger prints the level for each message. + * Default is true. + */ + static bool printLevel_; + + /* + * If the logger prints the end line for each message. + * Default is true. + */ + static bool printEndline_; + + /* + * If the logger prints text with color. + * Default is true. + */ + static bool printColored_; + + /* + * If the logger prints where the message is logged (fileName::function():line). + * Default is true. + */ + static bool printWhere_; + + /* + * If the logger prints the full path of the source file + * where the message is written. Only works when + * "printWhere_" is true. + * Default is false. + */ + static bool printWhereFullPath_; + + /* + * If the logger limit the size of the "where" path to + * characters. If the path is over 8 characters, a "~" + * is added. Only works when "printWhereFullPath_" is false. + * Default is false. + */ + static bool limitWhereLength_; + + /* + * The type of the logger. + */ + static Type type_; + + /* + * The severity of the log. + */ + static Level level_; + + /* + * The severity at which the application exits. + * Note : A FATAL level will always exit whatever the level specified here. + */ + static Level exitLevel_; + + /* + * The severity at which the message is also sent in a ULogEvent. + */ + static Level eventLevel_; + + static const char * levelName_[5]; + + /* + * Mutex used when accessing public functions. + */ + static UMutex loggerMutex_; + + /* + * If the logger prints messages only when ULogger::flush() is called. + * Default is false. + */ + static bool buffered_; + + static std::string bufferedMsgs_; + + /* + * State attribute. This state happens when an exit level + * message is received. + * Messages received during this state are not logged. + * @see exitLevel_ + */ + static bool exitingState_; +}; + +#endif // ULOGGER_H diff --git a/src/utilite/UMutex.h b/src/utilite/UMutex.h new file mode 100644 index 00000000..bc4a088b --- /dev/null +++ b/src/utilite/UMutex.h @@ -0,0 +1,179 @@ +/* +* utilite is a cross-platform library with +* useful utilities for fast and small developing. +* Copyright (C) 2010 Mathieu Labbe +* +* utilite is free library: you can redistribute it and/or modify +* it under the terms of the GNU Lesser General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* utilite is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU Lesser General Public License for more details. +* +* You should have received a copy of the GNU Lesser General Public License +* along with this program. If not, see . +*/ + +#ifndef UMUTEX_H +#define UMUTEX_H + +#include + +#ifdef WIN32 + #include "utilite/UWin32.h" +#else + #include +#endif + + +/** + * A mutex class. + * + * On a lock() call, the calling thread is blocked if the + * UMutex was previously locked by another thread. It is unblocked when unlock() is called. + * + * On Unix (not yet tested on Windows), UMutex is recursive: the same thread can + * call multiple times lock() without being blocked. + * + * Example: + * @code + * UMutex m; // Mutex shared with another thread(s). + * ... + * m.lock(); + * // Data is protected here from the second thread + * //(assuming the second one protects also with the same mutex the same data). + * m.unlock(); + * + * @endcode + * + * @see USemaphore + */ +class UMutex +{ + +public: + + /** + * The constructor. + */ + UMutex() + { +#ifdef WIN32 + InitializeCriticalSection(&C); +#else + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_settype(&attr,PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&M,&attr); + pthread_mutexattr_destroy(&attr); +#endif + } + + virtual ~UMutex() + { +#ifdef WIN32 + DeleteCriticalSection(&C); +#else + pthread_mutex_unlock(&M); pthread_mutex_destroy(&M); +#endif + } + + /** + * Lock the mutex. + */ + int lock() const + { +#ifdef WIN32 + EnterCriticalSection(&C); return 0; +#else + return pthread_mutex_lock(&M); +#endif + } + +#ifdef WIN32 + #if(_WIN32_WINNT >= 0x0400) + int lockTry() const + { + return (TryEnterCriticalSection(&C)?0:EBUSY); + } + #endif +#else + int lockTry() const + { + return pthread_mutex_trylock(&M); + } +#endif + + /** + * Unlock the mutex. + */ + int unlock() const + { +#ifdef WIN32 + LeaveCriticalSection(&C); return 0; +#else + return pthread_mutex_unlock(&M); +#endif + } + + private: +#ifdef WIN32 + mutable CRITICAL_SECTION C; +#else + mutable pthread_mutex_t M; +#endif + void operator=(UMutex &M) {} + UMutex( const UMutex &M ) {} +}; + +/** + * Automatically lock the referenced mutex on constructor and unlock mutex on destructor. + * + * Example: + * @code + * UMutex m; // Mutex shared with another thread(s). + * ... + * int myMethod() + * { + * UScopeMutex sm(m); // automatically lock the mutex m + * if(cond1) + * { + * return 1; // automatically unlock the mutex m + * } + * else if(cond2) + * { + * return 2; // automatically unlock the mutex m + * } + * return 0; // automatically unlock the mutex m + * } + * + * @endcode + * + * @see UMutex + */ +class UScopeMutex +{ +public: + UScopeMutex(const UMutex & mutex) : + mutex_(mutex) + { + mutex_.lock(); + } + // backward compatibility + UScopeMutex(UMutex * mutex) : + mutex_(*mutex) + { + mutex_.lock(); + } + ~UScopeMutex() + { + mutex_.unlock(); + } +private: + const UMutex & mutex_; +}; + +#endif // UMUTEX_H diff --git a/src/utilite/UWin32.h b/src/utilite/UWin32.h new file mode 100644 index 00000000..19bb1048 --- /dev/null +++ b/src/utilite/UWin32.h @@ -0,0 +1,64 @@ +///////////////////////////////////////////////////////////////////// +// Written by Phillip Sitbon +// Copyright 2003 +// +// Win32.h +// - Windows includes +// +///////////////////////////////////////////////////////////////////// +#ifndef _U_Win32_ +#define _U_Win32_ + +#include "utilite/UtiLiteExp.h" + + #if !defined(_WINDOWS_) + // WIN32 Excludes + #ifdef WIN32_LEAN_AND_MEAN + # define VC_EXTRALEAN + # define WIN32_LEAN_AND_MEAN + # define _PRSHT_H_ + # define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_ + # define NOVIRTUALKEYCODES // VK_* + # define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_* + # define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_* + # define NOSYSMETRICS // SM_* + # define NOMENUS // MF_* + # define NOICONS // IDI_* + # define NOKEYSTATES // MK_* + # define NOSYSCOMMANDS // SC_* + # define NORASTEROPS // Binary and Tertiary raster ops + # define NOSHOWWINDOW // SW_* + # define OEMRESOURCE // OEM Resource values + # define NOATOM // Atom Manager routines + # define NOCLIPBOARD // Clipboard routines + # define NOCOLOR // Screen colors + # define NOCTLMGR // Control and Dialog routines + # define NODRAWTEXT // DrawText() and DT_* + # define NOGDI // All GDI defines and routines + # define NOKERNEL // All KERNEL defines and routines + # define NOUSER // All USER defines and routines + # define NONLS // All NLS defines and routines + # define NOMB // MB_* and MessageBox() + # define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines + # define NOMETAFILE // typedef METAFILEPICT + # define NOMINMAX // Macros min(a,b) and max(a,b) + # define NOMSG // typedef MSG and associated routines + # define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_* + # define NOSCROLL // SB_* and scrolling routines + # define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc. + # define NOSOUND // Sound driver routines + # define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines + # define NOWH // SetWindowsHook and WH_* + # define NOWINOFFSETS // GWL_*, GCL_*, associated routines + # define NOCOMM // COMM driver routines + # define NOKANJI // Kanji support stuff. + # define NOHELP // Help engine interface. + # define NOPROFILER // Profiler interface. + # define NODEFERWINDOWPOS // DeferWindowPos routines + # define NOMCX // Modem Configuration Extensions + #endif // WIN32_LEAN_AND_MEAN + // + # include + #endif + +#endif // !_U_Win32_