sift_gpu_wrapper.cpp
Go to the documentation of this file.
00001 /* This file was inspired by RGBDSLAM.
00002   */
00003 
00004 //#ifdef USE_SIFT_GPU
00005 #include "rgbd_registration/sift_gpu_wrapper.h"
00006 #include <GL/gl.h>
00007 #include <iostream>
00008 #include <ros/ros.h>
00009 #include <stdio.h>
00010 #include "rgbd_registration/parameter_server.h"
00011 
00012 using namespace cv;
00013 
00014 SiftGPUWrapper* SiftGPUWrapper::instance = NULL;
00015 
00016 SiftGPUWrapper::SiftGPUWrapper() {
00017     error = false;
00018     siftgpu = new SiftGPU();
00019 
00020 #if defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 1
00021     char method[] = {"-cuda"};
00022 #elif defined(SIFT_GPU_MODE) and SIFT_GPU_MODE == 2
00023     char method[] = {"-glsl"};
00024 #endif
00025 
00026     //sprintf(method, "%s", ParameterServer::instance()->get<bool>("cuda_available") ? "-cuda" : "-glsl");
00027     int max_features = ParameterServer::instance()->get<int>("max_keypoints");
00028     char max_feat_char[10];
00029     sprintf(max_feat_char, "%d", max_features);
00030     //ROS_INFO("Max_feat_char %s", max_feat_char);
00031     char subpixelKey[] = {"-s"};
00032     char subpixelValue[] = {"1"};
00033     char max_flag[] = {"-tc2"};
00034     //char verbosity[] = {"-v0"};//nothing but errors
00035     //char * argv[] = {method, "-t", "10", subpixelKey, subpixelValue, max_flag, max_feat_char};
00036     char first_octave[] = {"-fo"};
00037     char first_octave_val[] = {"-1"};
00038     char * argv[] = {method,  subpixelKey, subpixelValue, max_flag, max_feat_char, first_octave, first_octave_val};//, "-t", "0.005"};
00039     siftgpu->ParseParam(7, argv);
00040 
00041     if (siftgpu->CreateContextGL() != SiftGPU::SIFTGPU_FULL_SUPPORTED) {
00042         ROS_ERROR("Can't create OpenGL context! SiftGPU cannot be used.");
00043         error = true;
00044     }
00045 
00046     data = (unsigned char*) malloc(imageWidth * imageHeight);
00047 
00048     isMatcherInitialized = false;
00049 }
00050 
00051 SiftGPUWrapper::~SiftGPUWrapper() {
00052     delete siftgpu;
00053     if (isMatcherInitialized) delete matcher;
00054     instance = NULL;
00055     if (data != NULL) {
00056         free(data);
00057         data = NULL;
00058     }
00059 }
00060 
00061 void SiftGPUWrapper::destroyInstance() {
00062     delete instance;
00063 }
00064 SiftGPUWrapper* SiftGPUWrapper::getInstance() {
00065     if (instance == NULL) {
00066         ROS_INFO("Create Instance");
00067         instance = new SiftGPUWrapper();
00068     }
00069     return instance;
00070 }
00071 
00072 void SiftGPUWrapper::detect(const cv::Mat& image, cv::vector<cv::KeyPoint>& keypoints, std::vector<float>& descriptors, const Mat& mask) const {
00073     if (error) {
00074         keypoints.clear();
00075         ROS_FATAL("SiftGPU cannot be used. Detection of keypoints failed");
00076     }
00077 
00078     //get image
00079     cvMatToSiftGPU(image, data);
00080 
00081     int num_features = 0;
00082     SiftGPU::SiftKeypoint* keys = 0;
00083 
00084     ROS_DEBUG("SIFTGPU: cols: %d, rows: %d", image.cols, image.rows);
00085     if (siftgpu->RunSIFT(image.cols, image.rows, data, GL_LUMINANCE, GL_UNSIGNED_BYTE)) {
00086         num_features = siftgpu->GetFeatureNum();
00087         ROS_INFO("Number of features found: %i", num_features);
00088         keys = new SiftGPU::SiftKeypoint[num_features];
00089         descriptors.resize(128 * num_features);
00090         //descriptors = new float[128 * num_features];
00091         siftgpu->GetFeatureVector(&keys[0], &descriptors[0]);
00092     } else {
00093         ROS_WARN("SIFTGPU->RunSIFT() failed!");
00094     }
00095 
00096     //copy to opencv structure
00097     keypoints.clear();
00098     for (int i = 0; i < num_features; ++i) {
00099         KeyPoint key(keys[i].x, keys[i].y, keys[i].s, keys[i].o);
00100         keypoints.push_back(key);
00101     }
00102 
00103     //  FILE *fp = fopen("bla.pgm", "w");
00104     //  WritePGM(fp, data, image.cols, image.rows);
00105     //  fclose(fp);
00106 
00107 }
00108 
00109 int SiftGPUWrapper::match(
00110         const std::vector<float>& descriptors1,
00111         int num1,
00112         const std::vector<float>& descriptors2,
00113         int num2,
00114         std::vector<cv::DMatch>* matches) {
00115     if (!isMatcherInitialized)
00116         initializeMatcher();
00117 
00118     float sumDistances = 0;
00119 
00120     matcher->SetDescriptors(0, num1, &descriptors1[0]);
00121     matcher->SetDescriptors(1, num2, &descriptors2[0]);
00122 
00123     int (*match_buf)[2] = new int[num1][2];
00124     int number = matcher->GetSiftMatch(num1, match_buf, 0.9, 0.9);
00125 
00126     if (matches->size() != 0) {
00127         ROS_WARN("Clearing matches vector!");
00128         matches->clear();
00129     }
00130 
00131     cv::DMatch match;
00132     int counter = 0;
00133 
00134     for (int i = 0; i < number; i++) {
00135         match.queryIdx = match_buf[i][0];
00136         match.trainIdx = match_buf[i][1];
00137 
00138         //only use matches with indices != 0 (opengl context problem may sometimes happen)
00139         if (match.queryIdx == 0 || match.trainIdx == 0) {
00140             counter++;
00141         }
00142 
00143         if (counter > 0.5 * number) {
00144             matches->clear();
00145             sumDistances = 0;
00146             ROS_ERROR("Matches bad due to context error");
00147             break;
00148         }
00149 
00150         float sum = 0;
00151         for (int j = 0; j < 128; j++) {
00152             float a = descriptors1[match.queryIdx * 128 + j] - descriptors2[match.trainIdx * 128 + j];
00153             sum += a * a;
00154         }
00155 
00156         match.distance = sqrt(sum);
00157         sumDistances += match.distance;
00158         matches->push_back(match);
00159         ROS_DEBUG("Matched Features %d and %d with distance of %f. Sum: %f", match.queryIdx, match.trainIdx, match.distance, sumDistances);
00160     }
00161 
00162     delete[] match_buf;
00163 
00164     return sumDistances;
00165 }
00166 
00167 void SiftGPUWrapper::initializeMatcher() {
00168     matcher = CreateNewSiftMatchGPU(4096);
00169     if (!matcher->VerifyContextGL()) {
00170         ROS_FATAL("Can't create OpenGL context! SiftGPU Matcher cannot be used.");
00171         error = true;
00172         return;
00173     }
00174     ROS_INFO("matcher - ok");
00175     isMatcherInitialized = true;
00176 }
00177 
00178 void SiftGPUWrapper::cvMatToSiftGPU(const Mat& image,
00179         unsigned char* siftImage) const {
00180     Mat tmp;
00181     image.convertTo(tmp, CV_8U);
00182     for (int y = 0; y < tmp.rows; ++y) {
00183         for (int x = 0; x < tmp.cols; ++x) {
00184             siftImage[y * tmp.cols + x] = tmp.at<unsigned char> (y, x);
00185         }
00186     }
00187 }
00188 
00189 void SiftGPUWrapper::writePGM(FILE *fp, unsigned char* data, int width, int height)
00190 {
00191     int val;
00192     fprintf(fp, "P5\n%d %d\n255\n", width, height);
00193 
00194     for (int y = 0; y < height; y++) {
00195         for (int x = 0; x < width; x++) {
00196             val = (int) (/*255.0 */data[y * width + x]);
00197             if (x == 0) val = 255;
00198             if (y == 0) val = 255;
00199             fputc(MAX(0, MIN(255, val)), fp);
00200         }
00201     }
00202 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Thu May 23 2013 15:36:53