DWORD WINAPI Thread_ExtractThumbnailImage(LPVOID lpParameter)
{
CDisplayWindow *pdw = NULL;
pdw = (CDisplayWindow *)((ThumbnailEntry_t *)lpParameter)->pdw;
CoInitializeEx(0,COINIT_APARTMENTTHREADED);
SetThreadPriority(GetCurrentThread(),THREAD_PRIORITY_BELOW_NORMAL);
pdw->ExtractThumbnailImageInternal((ThumbnailEntry_t *)lpParameter);
/* TODO: Erase the item. */
//g_ThumbnailEntries.erase();
CoUninitialize();
return 0;
}
CDisplayWindow* BibleTime::createWriteDisplayWindow(CSwordModuleInfo* module, const QString& key, const CDisplayWindow::WriteWindowType& type) {
qApp->setOverrideCursor( QCursor(Qt::WaitCursor) );
QList<CSwordModuleInfo*> modules;
modules.append(module);
CDisplayWindow* displayWindow = CDisplayWindowFactory::createWriteInstance(modules, m_mdi, type);
if ( displayWindow ) {
displayWindow->init();
if (m_mdi->subWindowList().count() == 0)
displayWindow->showMaximized();
else
displayWindow->show();
displayWindow->lookupKey(key);
}
qApp->restoreOverrideCursor();
return displayWindow;
}
/** Creates a new presenter in the MDI area according to the type of the module. */
CDisplayWindow* BibleTime::createReadDisplayWindow(QList<CSwordModuleInfo*> modules, const QString& key) {
qApp->setOverrideCursor( QCursor(Qt::WaitCursor) );
qDebug() << "BibleTime::createReadDisplayWindow(QList<CSwordModuleInfo*> modules, const QString& key)";
CDisplayWindow* displayWindow = CDisplayWindowFactory::createReadInstance(modules, m_mdi);
if ( displayWindow ) {
displayWindow->init();
if (m_mdi->subWindowList().count() == 0)
displayWindow->showMaximized();
else
displayWindow->show();
// if (!key.isEmpty())
displayWindow->lookupKey(key);
}
// We have to process pending events here, otherwise displayWindow is not fully painted
qApp->processEvents();
// Now all events, including mouse clicks for the displayWindow have been handled
// and we can let the user click the same module again
//m_bookshelfPage->unfreezeModules(modules);
qApp->restoreOverrideCursor();
return displayWindow;
}
//Shows the video in the "win" window.
void videowindow(data_st* data)
{
CObservationImagePtr lastimg;
while(data->showvideo)
{
data->Robot.getLastImage2(lastimg);
if(lastimg) //Avoid NULL image
win.showImage(lastimg->image);
mrpt::system::sleep(50);
}
}
/*---------------------------------------------------------------
AlignPDF_correlation
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
void* info)
{
MRPT_UNUSED_PARAM(initialEstimationPDF);
MRPT_UNUSED_PARAM(info);
MRPT_START
//#define CORRELATION_SHOW_DEBUG
CTicTac* tictac = nullptr;
// Asserts:
// -----------------
ASSERT_(mm1->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
ASSERT_(mm2->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
const auto* m1 = static_cast<const COccupancyGridMap2D*>(mm1);
const auto* m2 = static_cast<const COccupancyGridMap2D*>(mm2);
ASSERT_(m1->getResolution() == m2->getResolution());
if (runningTime)
{
tictac = new CTicTac();
tictac->Tic();
}
// The PDF to estimate:
// ------------------------------------------------------
CPosePDFGaussian::Ptr PDF = mrpt::make_aligned_shared<CPosePDFGaussian>();
// Determine the extension to compute the correlation into:
// ----------------------------------------------------------
float phiResolution = DEG2RAD(0.2f);
float phiMin = -M_PIf + 0.5f * phiResolution;
float phiMax = M_PIf;
// Compute the difference between maps for each (u,v) pair!
// --------------------------------------------------------------
float phi;
float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
COccupancyGridMap2D map2_mod;
CImage map1_img, map2_img;
float currentMaxCorr = -1e6f;
m1->getAsImage(map1_img);
map2_mod.setSize(
m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(),
m1->getResolution());
size_t map2_lx = map2_mod.getSizeX();
size_t map2_ly = map2_mod.getSizeY();
CMatrix outCrossCorr, bestCrossCorr;
float map2width_2 = 0.5f * (map2_mod.getXMax() - map2_mod.getXMin());
#ifdef CORRELATION_SHOW_DEBUG
CDisplayWindow* win = new CDisplayWindow("corr");
CDisplayWindow* win2 = new CDisplayWindow("map2_mod");
#endif
// --------------------------------------------------------
// Use FFT-based correlation for each orientation:
// --------------------------------------------------------
for (phi = phiMin; phi < phiMax; phi += phiResolution)
{
// Create the displaced map2 grid, for the size of map1:
// --------------------------------------------------------
CPose2D v2(
pivotPt_x - cos(phi) * map2width_2,
pivotPt_y - sin(phi) * map2width_2,
phi); // Rotation point: the centre of img1:
CPoint2D v1, v3;
v2 = CPose2D(0, 0, 0) - v2; // Inverse
for (unsigned int cy2 = 0; cy2 < map2_ly; cy2++)
{
COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
for (unsigned int cx2 = 0; cx2 < map2_lx; cx2++)
{
v3 = v2 + CPoint2D(map2_mod.idx2x(cx2), map2_mod.idx2y(cy2));
*row++ = m2->p2l(m2->getPos(v3.x(), v3.y()));
}
}
map2_mod.getAsImage(map2_img);
// map2_img.saveToBMP("__debug_map2mod.bmp");
// Compute the correlation:
map1_img.cross_correlation_FFT(
map2_img, outCrossCorr, -1, -1, -1, -1,
127, // Bias to be substracted
127 // Bias to be substracted
);
float corrPeak = outCrossCorr.maxCoeff();
printf("phi = %fdeg \tmax corr=%f\n", RAD2DEG(phi), corrPeak);
// Keep the maximum:
if (corrPeak > currentMaxCorr)
{
currentMaxCorr = corrPeak;
bestCrossCorr = outCrossCorr;
PDF->mean.phi(phi);
}
#ifdef CORRELATION_SHOW_DEBUG
outCrossCorr.adjustRange(0, 1);
CImage aux(outCrossCorr, true);
win->showImage(aux);
win2->showImage(map2_img);
std::this_thread::sleep_for(5ms);
#endif
} // end for phi
if (runningTime)
{
*runningTime = tictac->Tac();
delete tictac;
}
bestCrossCorr.normalize(0, 1);
CImage aux(bestCrossCorr, true);
aux.saveToFile("_debug_best_corr.png");
#ifdef CORRELATION_SHOW_DEBUG
delete win;
delete win2;
#endif
// Transform the best corr matrix peak into coordinates:
CMatrix::Index uMax, vMax;
currentMaxCorr = bestCrossCorr.maxCoeff(&uMax, &vMax);
PDF->mean.x(m1->idx2x(uMax));
PDF->mean.y(m1->idx2y(vMax));
return PDF;
// Done!
MRPT_END
}
// ------------------------------------------------------
// TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
// Set your rawlog file name
if (!mrpt::system::fileExists(RAWLOG_FILE))
THROW_EXCEPTION_CUSTOM_MSG1("Rawlog file does not exist: %s",RAWLOG_FILE.c_str())
CActionCollectionPtr action;
CSensoryFramePtr observations;
size_t rawlogEntry = 0;
//bool end = false;
CDisplayWindow wind;
// Set relative path for externally-stored images in rawlogs:
string rawlog_images_path = extractFileDirectory( RAWLOG_FILE );
rawlog_images_path+="/Images";
CImage::IMAGES_PATH_BASE = rawlog_images_path; // Set it.
CFileGZInputStream rawlogFile( RAWLOG_FILE );
for (;;)
{
if (os::kbhit())
{
char c = os::getch();
if (c==27)
break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
cout << "Reading act/oct pair from rawlog..." << endl;
if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )
break; // file EOF
// CAMERA............
// Get CObservationStereoImages
CObservationStereoImagesPtr sImgs;
CObservationImagePtr Img;
sImgs = observations->getObservationByClass<CObservationStereoImages>();
if (!sImgs)
{
Img = observations->getObservationByClass<CObservationImage>();
if(!Img)
continue;
}
CPose3D cameraPose; // Get Camera Pose (B) (CPose3D)
CMatrixDouble33 K; // Get Calibration matrix (K)
sImgs ? sImgs->getSensorPose( cameraPose ) : Img->getSensorPose( cameraPose );
K = sImgs ? sImgs->leftCamera.intrinsicParams : Img->cameraParams.intrinsicParams;
// LASER.............
// Get CObservationRange2D
CObservation2DRangeScanPtr laserScan = observations->getObservationByClass<CObservation2DRangeScan>();
if (!laserScan) continue;
// Get Laser Pose (A) (CPose3D)
CPose3D laserPose;
laserScan->getSensorPose( laserPose );
if (abs(laserPose.yaw())>DEG2RAD(90)) continue; // Only front lasers
// Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
CPoint3D point;
CSimplePointsMap mapa;
mapa.insertionOptions.minDistBetweenLaserPoints = 0;
observations->insertObservationsInto( &mapa ); // <- The map contains the pose of the points (P1)
// Get the points into the map
vector<float> X, Y, Z;
vector<float>::iterator itX, itY, itZ;
mapa.getAllPoints(X,Y,Z);
unsigned int imgW = sImgs? sImgs->imageLeft.getWidth() : Img->image.getWidth();
unsigned int imgH = sImgs? sImgs->imageLeft.getHeight() : Img->image.getHeight();
//unsigned int idx = 0;
vector<float> imgX,imgY;
vector<float>::iterator itImgX,itImgY;
imgX.resize( X.size() );
imgY.resize( Y.size() );
CImage image;
image = sImgs ? sImgs->imageLeft : Img->image;
// Get pixels in the image:
// Pimg = (kx,ky,k)^T = K(I|0)*P2
// Main loop
for( itX = X.begin(), itY = Y.begin(), itZ = Z.begin(), itImgX = imgX.begin(), itImgY = imgY.begin();
itX != X.end();
itX++, itY++, itZ++, itImgX++, itImgY++)
{
// Coordinates Transformation
CPoint3D pLaser(*itX,*itY,*itZ);
CPoint3D pCamera(pLaser-cameraPose);
if( pCamera.z() > 0 )
{
*itImgX = - K(0,0)*((pCamera.x())/(pCamera.z())) + K(0,2);
*itImgY = - K(1,1)*((pCamera.y())/(pCamera.z())) + K(1,2);
if( *itImgX > 0 && *itImgX < imgW && *itImgY > 0 && *itImgY < imgH )
image.filledRectangle(*itImgX-1,*itImgY-1,*itImgX+1,*itImgY+1,TColor(255,0,0));
} // end if
} // end for
action.clear_unique();
observations.clear_unique();
wind.showImage(image);
mrpt::system::sleep(50);
}; // end for
mrpt::system::pause();
}
/*--------------------------------
| MAIN |
---------------------------------*/
int main()
{
//VAR
char decision; //to read from keyboard
data_st data;
bool output=false;
data.showvideo=false;
string response, errormsg;
mrpt::system::TThreadHandle screen_hd, featuring_hd, matching_hd, fm_hd;
int *a_enc;
int act=0; //Action
int dir=0; //Direction
int speed=3; //Speed
data.matched=false; //this var checks if there are extracted features
data.features_taken=false, data.matching_done=false, data.fm_calculated=false;
//string line;
//fstream f_harris, f_harris_b;
ofstream matRT;
CImage Picture;
std::vector<double> distortion2;
CMatrix distortion_matrix2(1,4), intrinsic_matrix2(3,3);
string imgname;
string MenuJoystick=
"---------------------------Joystick Controls---------------------------\n\n"
"|1->Turn to the left 2->Turn to the right |\n"
"|3->Head to the middle 4->Head down 3&4->Head up |\n"
"|1&2->Go Home Up&3->Go Home and Dock 1&3->Get Report |\n"
"-----------------------------------------------------------------------\n";
string MenuKeyboard=
"---------------------------Keyboard Controls---------------------------\n"
"|MOVEMENT CAMERA PATHS |\n"
"|'w'->up 'c'->Middle 'r'->Start |\n"
"|'s'->down 'z'->Up 't'->Stop/Save |\n"
"|'a'->left 'x'->Down 'y'->Show |\n"
"|'d'->up 'u'->Delete |\n"
"|'q'->Move left 'i'->Run Ford |\n"
"|'e'->move Right 'o'->Run Back |\n"
"|'g'->Move to target position |\n"
"| |\n"
"|EXTRA |\n"
"|'-'->-Speed '+'->+Speed 'v'->Video |\n"
"|'h'->Gohome 'j'->menuJY 'r'->menuKB |\n"
"|'l'->ShowPosition 'n'->ShowEnc 'k'->Get Report |\n"
"|'f'->Ext Features 'b'->Match Feat '*'->FMatrix |\n"
"| '.'->EXIT |\n"
"-----------------------------------------------------------------------\n\n";
//Code START here
try
{
//initialization of Rovio Values (Ip, user and password)
data.Robot.Initialize(errormsg);
if(errormsg.empty())
{
cout<<"What do you want from the robot?\n\n";
cout<<MenuKeyboard<<endl;
//cout<<MenuJoystick<<endl;
//Calling Joystick in other thread
//createThread(JoystickControl,&data);
do{
decision=mrpt::system::os::getch();
switch(decision)
{
//Movement control
case 'w':
act=18;dir=1;break;
case 's':
act=18;dir=2;break;
case 'q':
act=18;dir=3;break;
case 'e':
act=18;dir=4;break;
case 'a':
act=18;dir=5;break;
case 'd':
act=18;dir=6;break;
//camera head movement
case 'z':
act=18;dir=11;break;
case 'x':
act=18;dir=12;break;
case 'c':
act=18;dir=13;break;
//Paths
case 'r': //start
act=2;
cout << "Recording Path" << endl; break;
case 't': //stop and save path
act=4;
cout << "Path Saved" << endl; break;
case 'y': //GetPath
act=6;
cout << "Getting saved Paths" << endl;
output=true; break;
case 'u': //delete path
act=5;
cout << "Path deleted" << endl; break;
case 'i': //fordward
act=7;
cout << "Running path Fordward mode" << endl; break;
case 'o': //backward
act=8;
cout << "Running path Backward mode" << endl; break;
//Speed control
case '-':
act=0;
if (speed<=7)
speed+=2;
cout << "Speed: "<< speed <<endl;
break;
case '+':
act=0;
if(speed>=3)
speed-=2;
cout << "Speed: "<< speed <<endl;
break;
//Camera On/Off
case 'v':
act=0;
if(!data.showvideo) //open the window
{
cout<<"VIDEO ON"<<endl;
data.showvideo=!data.showvideo;
screen_hd=createThread(videowindow, &data);
}
else //close the window
{
cout<<"VIDEO OFF"<<endl;
data.showvideo=!data.showvideo; // this kill the thread
joinThread(screen_hd); //Wait till thread finish
win.~CDisplayWindow();
}
break;
case 'h':
cout << "GOING HOME" << endl;
act=13;break;
case 'j':
act=0;
cout<<MenuJoystick<<endl;break;
case 'm':
act=0;
cout<<MenuKeyboard<<endl;break;
case 'k':
act=1; output=true;
cout <<"--------------------------REPORT------------------------" << endl;
break;
case 'l': //Print the position
act=0; output=false;
showPosition(data);
break;
case 'g': //Go to Target Position
goThere(data, response, errormsg);
break;
case 'n': //Print the encoder's values.
act=0; output=false;
a_enc=data.Robot.getEncoders();
print_Encoders(a_enc);
break;
case 'f': //Take Features
act=0; output=false;
data.features_taken=false; //this var checks if the function TestExtractFeatures has finished
featuring_hd=createThread(TestExtractFeatures, &data);
break;
case 'b': //Match Features without epipolar restriction
act=0; output=false;
data.matching_done=false; //this var checks if the function TestExtractFeatures has finished
data.matching_type=complete_match;
matching_hd=createThread(matching, &data);
break;
case '1': //Change the matching to complete matching, get all the correspondences using epipolar restriction
act=0; output=false;
data.matching_done=false; //this var checks if the function TestExtractFeatures has finished
data.matching_type=epipolar_match;
matching_hd=createThread(matching, &data);
break;
case '*': //Calculate Fundamental Matrix
act=0; output=false;
data.fm_calculated=false; //this var checks if the function TestExtractFeatures has finished
fm_hd=createThread(getFMat, &data);
break;
case '/': //Calculate Fundamental Matrix
act=0; output=false;
data.fm_calculated=false; //this var checks if the function TestExtractFeatures has finished
fm_hd=createThread(getFMat_from_txt, &data);
break;
case '2': //Calculate the Translation & Rotation Matrix for the camera
act=0; output=false;
getRTMat(&data);
matRT.open("RT.txt");
cout<<"\nRT-Matrix\n\n";
cout<<data.RT<< endl;
matRT << data.RT.inMatlabFormat() << endl;
matRT.close();
break;
case '3': //Get the camera's intrinsic matrix
act=0; output=false;
data.intrinsic_matrix.loadFromTextFile("intrinsic_matrix.txt");
cout<<data.intrinsic_matrix;
break;
case 'p': //Take Picture and show it in a window
act=0; output=false;
data.Robot.captureImageRect(Picture);
win.showImage(Picture);
break;
case '9': //Extract features from a stream and match them
act=0; output=false;
FeaturesStream(&data);
break;
case '8': //Track features
act=0; output=false;
Tracking(&data);
break;
case '7': //Rectify images
intrinsic_matrix2.loadFromTextFile("intrinsic_matrix.txt");
distortion_matrix2.loadFromTextFile("distortion_matrix.txt");
distortion2.resize(4);
distortion2[0] = distortion_matrix2.get_unsafe(0,0);
distortion2[1] = distortion_matrix2.get_unsafe(0,1);
distortion2[2] = distortion_matrix2.get_unsafe(0,2);
distortion2[3] = distortion_matrix2.get_unsafe(0,3);
imgname="1.jpg";
for(int i=1;Picture.loadFromFile(imgname);i++)
{
Picture.rectifyImageInPlace(intrinsic_matrix2, distortion2);
Picture.saveToFile(imgname);
imgname=format("%i.jpg",i);
}
break;
default:
act=0; //No action is executed
}//end switch
//EXECUTION
if (act!=0)
{
data.Robot.Moving(act,dir,speed, response, errormsg);
//a_enc=data.Robot.getEncoders();
//print_Encoders(a_enc);
}
if(output)
{
cout << "RESPONSE->" << response <<endl;
output=false;
}
if (!errormsg.empty()){
cout <<"---------------------------------------------------" << endl;
cout << "ERROR->" <<errormsg <<endl;
}
if(data.features_taken) //End features thread
joinThread(featuring_hd);
if(data.matching_done) //End matching thread
joinThread(matching_hd);
if(data.fm_calculated) //End getFM thread
joinThread(fm_hd);
mrpt::system::sleep(10);
}while(decision!='.');
}//end if
else
mrpt::system::pause();
}//end try
catch(std::exception &e)
{
cerr << e.what() << endl;
return -1;
}
return 0;
}
