Statistics
| Branch: | Revision:

root / rgbdslam / src / sift_gpu_feature_detector.cpp @ 9240aaa3

History | View | Annotate | Download (4.62 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17
#ifdef USE_SIFT_GPU
18
#include "sift_gpu_feature_detector.h"
19
#include <GL/gl.h>
20
#include <iostream>
21
#include <ros/ros.h>
22
#include <stdio.h>
23
#include "parameter_server.h"
24

    
25
using namespace cv;
26

    
27
SiftGPUFeatureDetector* SiftGPUFeatureDetector::instance = NULL;
28

    
29
SiftGPUFeatureDetector::SiftGPUFeatureDetector() {
30
    error = false;
31
    siftgpu = new SiftGPU();
32

    
33
#if defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 1
34
    char method[] = {"-cuda"};
35
#elif defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 2
36
    char method[] = {"-glsl"};
37
#endif
38

    
39
    //sprintf(method, "%s", ParameterServer::instance()->get<bool>("cuda_available") ? "-cuda" : "-glsl");
40
    int max_features = ParameterServer::instance()->get<int>("max_keypoints");
41
    char max_feat_char[10];
42
    sprintf(max_feat_char, "%d", max_features);
43
    //ROS_INFO("Max_feat_char %s", max_feat_char);
44
    char subpixelKey[] = {"-s"};
45
    char subpixelValue[] = {"1"};
46
    char max_flag[] = {"-tc2"};
47
    //char verbosity[] = {"-v0"};//nothing but errors
48
    //char * argv[] = {method, "-t", "10", subpixelKey, subpixelValue, max_flag, max_feat_char};
49
    char first_octave[] = {"-fo"};
50
    char first_octave_val[] = {"-1"};
51
    char * argv[] = {method,  subpixelKey, subpixelValue, max_flag, max_feat_char, first_octave, first_octave_val};//, "-t", "0.005"};
52
    siftgpu->ParseParam(7, argv);
53

    
54
    if (siftgpu->CreateContextGL() != SiftGPU::SIFTGPU_FULL_SUPPORTED) {
55
        ROS_ERROR("Can't create OpenGL context! SiftGPU cannot be used.");
56
        error = true;
57
    }
58

    
59
    data = (unsigned char*) malloc(imageWidth * imageHeight);
60
}
61

    
62
SiftGPUFeatureDetector::~SiftGPUFeatureDetector() {
63
    delete siftgpu;
64
    instance = NULL;
65
    if (data != NULL) {
66
        free(data);
67
        data = NULL;
68
    }
69
}
70

    
71
void SiftGPUFeatureDetector::DestroyInstance() {
72
    delete instance;
73
}
74
SiftGPUFeatureDetector* SiftGPUFeatureDetector::GetInstance() {
75
    if (instance == NULL) {
76
        ROS_INFO("Create Instance");
77
        instance = new SiftGPUFeatureDetector();
78
    }
79
    return instance;
80
}
81

    
82
float* SiftGPUFeatureDetector::detect(const cv::Mat& image, cv::vector<cv::KeyPoint>& keypoints, const Mat& mask) const {
83
    if (error) {
84
        keypoints.clear();
85
        ROS_ERROR("SiftGPU cannot be used. Detection of keypoints failed");
86
        return 0;
87
    }
88

    
89
    //get image
90
    CVMatToSiftGPU(image, data);
91

    
92
    int num_features = 0;
93
    SiftGPU::SiftKeypoint* keys = 0;
94

    
95
    float* descriptors = 0;
96
    ROS_INFO("SIFTGPU: cols: %d, rows: %d", image.cols, image.rows);
97
    if (siftgpu->RunSIFT(image.cols, image.rows, data, GL_LUMINANCE, GL_UNSIGNED_BYTE)) {
98
        num_features = siftgpu->GetFeatureNum();
99
        ROS_INFO("Number of features found: %i", num_features);
100
        keys = new SiftGPU::SiftKeypoint[num_features];
101
        descriptors = new float[128 * num_features];
102
        siftgpu->GetFeatureVector(&keys[0], &descriptors[0]);
103
    } else {
104
        ROS_WARN("SIFTGPU->RunSIFT() failed!");
105
    }
106

    
107
    //copy to opencv structure
108
    keypoints.clear();
109
    for (int i = 0; i < num_features; ++i) {
110
        KeyPoint key(keys[i].x, keys[i].y, keys[i].s, keys[i].o);
111
        keypoints.push_back(key);
112
    }
113

    
114
    //        FILE *fp = fopen("bla.pgm", "w");
115
    //        WritePGM(fp, data, image.cols, image.rows);
116
    //        fclose(fp);
117

    
118
    return descriptors;
119
}
120

    
121
void SiftGPUFeatureDetector::CVMatToSiftGPU(const Mat& image,
122
        unsigned char* siftImage) const {
123
    Mat tmp;
124
    image.convertTo(tmp, CV_8U);
125
    for (int y = 0; y < tmp.rows; ++y) {
126
        for (int x = 0; x < tmp.cols; ++x) {
127
            siftImage[y * tmp.cols + x] = tmp.at<unsigned char> (y, x);
128
        }
129
    }
130
}
131

    
132
void SiftGPUFeatureDetector::WritePGM(FILE *fp, unsigned char* data, int width, int height)
133
{
134
    int val;
135
    fprintf(fp, "P5\n%d %d\n255\n", width, height);
136

    
137
    for (int y = 0; y < height; y++) {
138
        for (int x = 0; x < width; x++) {
139
            val = (int) (/*255.0 */data[y * width + x]);
140
            if (x == 0) val = 255;
141
            if (y == 0) val = 255;
142
            fputc(MAX(0, MIN(255, val)), fp);
143
        }
144
    }
145
}
146
#endif