Quadrotor

/* This file is part of RGBDSLAM.

 * 

 * RGBDSLAM is free software: you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation, either version 3 of the License, or

 * (at your option) any later version.

 * 

 * RGBDSLAM 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 General Public License for more details.

 * 

 * You should have received a copy of the GNU General Public License

 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.

 */

#ifdef USE_SIFT_GPU

#include "sift_gpu_feature_detector.h"

#include <GL/gl.h>

#include <iostream>

#include <ros/ros.h>

#include <stdio.h>

#include "parameter_server.h"

using namespace cv;

SiftGPUFeatureDetector* SiftGPUFeatureDetector::instance = NULL;

SiftGPUFeatureDetector::SiftGPUFeatureDetector() {

    error = false;

    siftgpu = new SiftGPU();

#if defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 1

    char method[] = {"-cuda"};

#elif defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 2

    char method[] = {"-glsl"};

#endif

    //sprintf(method, "%s", ParameterServer::instance()->get<bool>("cuda_available") ? "-cuda" : "-glsl");

    int max_features = ParameterServer::instance()->get<int>("max_keypoints");

    char max_feat_char[10];

    sprintf(max_feat_char, "%d", max_features);

    //ROS_INFO("Max_feat_char %s", max_feat_char);

    char subpixelKey[] = {"-s"};

    char subpixelValue[] = {"1"};

    char max_flag[] = {"-tc2"};

    //char verbosity[] = {"-v0"};//nothing but errors

    //char * argv[] = {method, "-t", "10", subpixelKey, subpixelValue, max_flag, max_feat_char};

    char first_octave[] = {"-fo"};

    char first_octave_val[] = {"-1"};

    char * argv[] = {method,  subpixelKey, subpixelValue, max_flag, max_feat_char, first_octave, first_octave_val};//, "-t", "0.005"};

    siftgpu->ParseParam(7, argv);

    if (siftgpu->CreateContextGL() != SiftGPU::SIFTGPU_FULL_SUPPORTED) {

        ROS_ERROR("Can't create OpenGL context! SiftGPU cannot be used.");

        error = true;

    }

    data = (unsigned char*) malloc(imageWidth * imageHeight);

}

SiftGPUFeatureDetector::~SiftGPUFeatureDetector() {

    delete siftgpu;

    instance = NULL;

    if (data != NULL) {

        free(data);

        data = NULL;

    }

}

void SiftGPUFeatureDetector::DestroyInstance() {

    delete instance;

}

SiftGPUFeatureDetector* SiftGPUFeatureDetector::GetInstance() {

    if (instance == NULL) {

        ROS_INFO("Create Instance");

        instance = new SiftGPUFeatureDetector();

    }

    return instance;

}

float* SiftGPUFeatureDetector::detect(const cv::Mat& image, cv::vector<cv::KeyPoint>& keypoints, const Mat& mask) const {

    if (error) {

        keypoints.clear();

        ROS_ERROR("SiftGPU cannot be used. Detection of keypoints failed");

        return 0;

    }

    //get image

    CVMatToSiftGPU(image, data);

    int num_features = 0;

    SiftGPU::SiftKeypoint* keys = 0;

    float* descriptors = 0;

    ROS_INFO("SIFTGPU: cols: %d, rows: %d", image.cols, image.rows);

    if (siftgpu->RunSIFT(image.cols, image.rows, data, GL_LUMINANCE, GL_UNSIGNED_BYTE)) {

        num_features = siftgpu->GetFeatureNum();

        ROS_INFO("Number of features found: %i", num_features);

        keys = new SiftGPU::SiftKeypoint[num_features];

        descriptors = new float[128 * num_features];

        siftgpu->GetFeatureVector(&keys[0], &descriptors[0]);

    } else {

        ROS_WARN("SIFTGPU->RunSIFT() failed!");

    }

    //copy to opencv structure

    keypoints.clear();

    for (int i = 0; i < num_features; ++i) {

        KeyPoint key(keys[i].x, keys[i].y, keys[i].s, keys[i].o);

        keypoints.push_back(key);

    }

    //        FILE *fp = fopen("bla.pgm", "w");

    //        WritePGM(fp, data, image.cols, image.rows);

    //        fclose(fp);

    return descriptors;

}

void SiftGPUFeatureDetector::CVMatToSiftGPU(const Mat& image,

        unsigned char* siftImage) const {

    Mat tmp;

    image.convertTo(tmp, CV_8U);

    for (int y = 0; y < tmp.rows; ++y) {

        for (int x = 0; x < tmp.cols; ++x) {

            siftImage[y * tmp.cols + x] = tmp.at<unsigned char> (y, x);

        }

    }

}

void SiftGPUFeatureDetector::WritePGM(FILE *fp, unsigned char* data, int width, int height)

{

    int val;

    fprintf(fp, "P5\n%d %d\n255\n", width, height);

    for (int y = 0; y < height; y++) {

        for (int x = 0; x < width; x++) {

            val = (int) (/*255.0 */data[y * width + x]);

            if (x == 0) val = 255;

            if (y == 0) val = 255;

            fputc(MAX(0, MIN(255, val)), fp);

        }

    }

}

#endif