i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged Audio branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-06-24 17:19:34 +00:00
parent 06fb556e78
commit 17b8e10ed8
111 changed files with 8370 additions and 9779 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
tools/Polar/CMakeLists.txt Normal file
View File

@@ -0,0 +1,27 @@
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${FFTW3F_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBRARIES}
${FFTW3F_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
ADD_EXECUTABLE(logPolar main.cpp)
TARGET_LINK_LIBRARIES(logPolar rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES( logPolar
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-logPolar)
INSTALL(TARGETS logPolar
RUNTIME DESTINATION bin COMPONENT runtime
LIBRARY DESTINATION lib COMPONENT devel
ARCHIVE DESTINATION lib COMPONENT devel)

144
tools/Polar/main.cpp Normal file
View File

@@ -0,0 +1,144 @@
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc_c.h>
#include <iostream>
#include <utilite/ULogger.h>
#include <utilite/UTimer.h>
#include <fftw3.h>
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/ColorTable.h"
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kDebug);
cv::Mat srcMat;
if( argc == 2)
{
srcMat=cvLoadImage(argv[1],1);
}
else
{
rtabmap::CameraVideo cam(0,0,false, 640, 480);
if(cam.init())
{
srcMat = cam.takeImage();
}
}
if(!srcMat.empty())
{
UTimer timer;
timer.start();
IplImage srfRef = srcMat;
IplImage * src = &srfRef;
// Log-polar transform
int radius = src->height < src->width ? src->height/2: src->width/2;
CvSize polarSize = cvSize(64, 128);
float M = polarSize.width/std::log(radius);
UDEBUG("src size=(%d,%d) radius=%d, M=%f", src->width, src->height, radius, M);
IplImage* polar = cvCreateImage( polarSize, 8, 3 );
IplImage* src2 = cvCreateImage( cvGetSize(src), 8, 3 );
cvLogPolar( src, polar, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
cvLogPolar( polar, src2, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
UDEBUG("logpolar time=%fs", timer.ticks());
// HSV transform
IplImage* hsv = cvCreateImage( cvGetSize(polar), IPL_DEPTH_8U, 3 );
cvCvtColor( polar, hsv, CV_BGR2HSV );
UDEBUG("bgr->hsv time=%fs", timer.ticks());
// Fetch H channel
cv::Mat hsvMat(hsv);
cv::vector<cv::Mat> channels;
split(hsvMat, channels);
cv::Mat hMat;
channels[0].convertTo(hMat, CV_32F);
hMat /= 255.0f;
UDEBUG("fetch h channel time=%fs", timer.ticks());
// DFT transform
cv::Mat dftMat;
cv::dft(hMat, dftMat, cv::DFT_ROWS);
UDEBUG("dft opencv time=%fs", timer.ticks());
// FFT transform
float in[hMat.cols];
fftwf_complex * out;
fftwf_plan p;
cv::Mat fftMat(hMat.rows, hMat.cols/2+1, CV_32F);
out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * fftMat.cols);
p = fftwf_plan_dft_r2c_1d(hMat.cols, in, out, 0);
UDEBUG("fft create plan time=%fs", timer.ticks());
if(p==0)
{
UFATAL("cannot create a plan");
}
for(int i=0; i<hMat.rows; ++i)
{
cv::Mat hRow = hMat(cv::Range(i,i+1), cv::Range(0,hMat.cols));
memcpy(in, hRow.data, sizeof(float)*hRow.cols);
fftwf_execute(p); /* repeat as needed */
for(int j=0; j<fftMat.cols; ++j)
{
cv::Mat fftRow = fftMat(cv::Range(i,i+1), cv::Range(0,fftMat.cols));
float re = (float)out[j][0];
float im = (float)out[j][1];
fftRow.at<float>(0,j) = sqrt(re*re+im*im); // TODO keep only real of the complex instead of the module ?
}
}
UDEBUG("fft time=%fs", timer.ticks());
fftwf_destroy_plan(p);
fftwf_free(out);
UDEBUG("fft cleanup time=%fs", timer.ticks());
//std::cout << dftMat << std::endl;
//std::cout << fftMat << std::endl;
//UDEBUG("hMat row=%d cols=%d", hMat.rows, hMat.cols);
//UDEBUG("hMat row=%d cols=%d, channels=%d", dftMat.rows, dftMat.cols, dftMat.channels());
IplImage * ind = cvCloneImage(src);
unsigned char * imageData = (unsigned char *)ind->imageData;
rtabmap::ColorTable colorTable(65536);
UDEBUG("widthStep=%d", ind->widthStep);
for(int i=0; i<ind->height; ++i)
{
for(int j=0; j<ind->width; ++j)
{
unsigned char & b = imageData[i*ind->widthStep+j*3+0];
unsigned char & g = imageData[i*ind->widthStep+j*3+1];
unsigned char & r = imageData[i*ind->widthStep+j*3+2];
int index = (int)colorTable.getIndex(r, g, b);
colorTable.getRgb(index, r, g , b);
}
}
cvNamedWindow( "log-original", 1 );
cvShowImage( "log-original", src );
cvNamedWindow( "log-polar", 1 );
cvShowImage( "log-polar", polar );
cvNamedWindow( "inverse log-polar", 1 );
cvShowImage( "inverse log-polar", src2 );
cvNamedWindow( "hsv", 1 );
cvShowImage( "hsv", hsv );
cvNamedWindow( "ind", 1 );
cvShowImage( "ind", ind );
UDEBUG("show time=%fs", timer.ticks());
cvWaitKey();
cvReleaseImage(&polar);
cvReleaseImage(&src2);
cvReleaseImage(&hsv);
cvReleaseImage(&ind);
}
return 0;
}