root / rgbdslam / src / sift_gpu_feature_detector.cpp @ 9240aaa3
History | View | Annotate | Download (4.62 KB)
1 | 9240aaa3 | Alex | /* 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 |