void preprocess(Mat &thresh_maze, Mat &color_maze, int size)
{
// Mat grid = Mat(maze.size(), CV_8UC1);
// imshow("original", maze);
// Preprocess the image
processing.preprocess_image(thresh_maze, thresh_maze);
processing.resize_to_max(thresh_maze, size);
processing.resize_to_max(color_maze, size);
}
/**
* @param grafEngine Graphic engine to show WSA animation
*/
void WSAMovie::animate(GraphicsEngine& grafEngine)
{
float fps, delay;
SDL_Rect dest;
int i;
SDL_Surface* frame;
ImageProc scaler;
frame = 0;
dest.w = header.width<<1;
dest.h = header.height<<1;
dest.x = (grafEngine.getWidth()-(header.width<<1))>>1;
dest.y = (grafEngine.getHeight()-(header.height<<1))>>1;
fps = static_cast<float>((1024.0 / (float) header.delta) * 1024.0);
delay = static_cast<float>((1.0 / fps) * 1000.0);
// Clear the screen
grafEngine.clearScreen();
// check
if (header.NumFrames == 0) {
return;
}
// For every frame: decode it, draw it and wait the delay
for (i = 0; i < header.NumFrames; i++)
{
// Decode the frame
frame = decodeFrame(i);
// Draw it to the screen
grafEngine.drawVQAFrame(scaler.scale(frame,1));
// Wait a delay
SDL_Delay((unsigned int)delay);
}
// Release the surface
SDL_FreeSurface(frame);
}
/** Play the vqamovie */
void VQAMovie::play()
{
SDL_Surface *frame, *cframe;
SDL_Rect dest;
SDL_Event esc_event;
Uint8 *sndbuf;
Uint16 sndlen;
int i;
static ImageProc scaler;
if( vqafile == NULL )
return;
dest.w = header.Width<<1;
dest.h = header.Height<<1;
dest.x = (pc::gfxeng->getWidth()-(header.Width<<1))>>1;
dest.y = (pc::gfxeng->getHeight()-(header.Height<<1))>>1;
/* clear the screen */
pc::gfxeng->clearScreen();
/* Seek to first frame/snd information of vqa */
vqafile->seekSet(offsets[0]);
/* init the sound - if there is no sound, DecodeSNDChunk returns 0 for sndlen and we continue*/
/* Is it safe to continue with sndlen set to zero? */
sndindex = 0;
sndsample = 0;
sndbuf = new Uint8[MAX_UNCOMPRESSED_SIZE];
memset(sndbuf, 0, MAX_UNCOMPRESSED_SIZE);
sndlen = DecodeSNDChunk(sndbuf);
pc::sfxeng->initVQAsound(sndbuf, sndlen);
/* create the frame to store the image in. */
frame = SDL_CreateRGBSurface(SDL_SWSURFACE, header.Width, header.Height, 8, 0, 0, 0, 0);
/* Initialise the scaler */
if( scaleVideo )
scaler.initVideoScale(frame, videoScaleQuality);
for( i = 0; i < header.NumFrames; i++ ) {
/* decode SND Chunk first */
sndlen = DecodeSNDChunk(sndbuf);
if(DecodeVQFRChunk(frame) == 1) {
/* Error decoding frame! We cannot continue, we must bail out */
break;
}
/* add the sound, when this function returns it's time to add the picture */
pc::sfxeng->addVQAsound(sndbuf, sndlen);
/* draw the frame */
if( scaleVideo ) {
cframe = scaler.scaleVideo(frame);
pc::gfxeng->drawVQAFrame(cframe);
} else {
pc::gfxeng->drawVQAFrame(frame);
}
//SDL_FreeSurface(cframe);
while ( SDL_PollEvent(&esc_event) ) {
if (esc_event.type == SDL_KEYDOWN) {
if (esc_event.key.state != SDL_PRESSED)
break;
if (esc_event.key.keysym.sym == SDLK_ESCAPE) {
i = header.NumFrames; /* set i high to break for loop*/
break;
}
}
} /* while */
} /* for */
pc::sfxeng->closeVQAsound();
SDL_FreeSurface(frame);
if( scaleVideo ) {
scaler.closeVideoScale();
}
delete[] sndbuf;
}
int main(int argc, char* argv[]) {
//Declaring and Initializing Variable and Objects
SP_CONFIG_MSG msg; //msg pointer
SPConfig config; //config struct instance
char* imagePath = (char*)malloc(sizeof(char)*MAXLEN);
SP_SPLIT_METHOD method;
SPPoint* resPoints; //temporary point array for each img
SPPoint* totalResPoints; //Point Array for all features
KDArray kdarray;
KDTreeNode root;
int sizeOfTotalFeat;
//Checking argument state
if (argc == 1) {
config = spConfigCreate(DEFAULT_FILE, &msg);
if (msg == SP_CONFIG_CANNOT_OPEN_FILE) {
printf(DEFAULT_ERROR_FILE_OPEN_MSG);
return 1; //exit(1)
} else if (msg != SP_CONFIG_SUCCESS)
return 1; //exit(1)
} else if (argc == 3 && strcmp(argv[1], "-c") == 0) {
config = spConfigCreate(argv[2], &msg);
if (msg == SP_CONFIG_CANNOT_OPEN_FILE) {
printf(ERROR_FILE_OPEN_MSG, argv[2]);
return 1; //exit(1)
} else if (msg != SP_CONFIG_SUCCESS)
return 1; //exit(1)
} else {
printf(INVALID_CMD_LINE_ERROR_MSG);
return 1; //exit(1)
}
//Declaring and Initializing Variable from configuration
int numOfImg = spConfigGetNumOfImages(config, &msg);
int numSimilarImg = spConfigGetSimilarImages(config, &msg);
int spKNN = spConfigGetKNN(config, &msg);
int dim = spConfigGetPCADim(config, &msg);
//Init Logger
char* loggerFile = (char*)malloc(sizeof(char)*MAXLEN);
SP_LOGGER_LEVEL loglevel = spConfigGetLoggerLevel(config);
msg = spConfigGetLoggerFilename(loggerFile,config);
if(msg==SP_CONFIG_INVALID_ARGUMENT){
spLoggerPrintError(CONFIG_FAIL_MSG,__FILE__,__func__,__LINE__);
}
if (strcmp(loggerFile, "stdout") == 0){
//Creating logger with std-output printing
spLoggerCreate(NULL,loglevel);
}
else
spLoggerCreate(loggerFile,loglevel);
//Init and allocate after logger created.
int* numOfFeats = (int*) malloc(sizeof(int)); //Temporary int pointer for the creating of .feat files
if(numOfFeats==NULL){
spLoggerPrintError(FAIL_ALOC_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
spLoggerPrintInfo(CONFIG_INIT_SUCCESS);
//Creating new ImageProc instance by using configuration.
ImageProc imageProc = ImageProc(config);
spLoggerPrintInfo(IMGPROC_INIT_SUCCESS_MSG);
/**
* Checking Extraction Mode State:
* if extractionMode=true-->creating .feat files
* else -->use feat files DB.
*/
if (spConfigIsExtractionMode(config, &msg)) {
if(msg != SP_CONFIG_SUCCESS){
spLoggerPrintError(INVALID_EXTRAC_ARG_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
for (int i = 0; i < numOfImg; i++) {
msg = spConfigGetImagePath(imagePath, config, i); //generate img path
if (msg != SP_CONFIG_SUCCESS){
spLoggerPrintError(INVALID_EXTRAC_ARG_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Extracting Image features for each image into SPPoint Array.
resPoints = imageProc.getImageFeatures(imagePath, i, numOfFeats); //get img feat for the feat file
if(resPoints==NULL){
spLoggerPrintError(FAIL_ALOC_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Auxilary function - extracting features values to .feat files.
msg = createFeatFiles(config, imagePath, i, numOfFeats, resPoints); //CHECKK HERE/ LIRON GET CALM
if(msg!=SP_CONFIG_SUCCESS){
spLoggerPrintError(FEAT_FIlLES_INIT_FAIL_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Destroying the SPPoint Array for reusing.
for (int j = 0; j < *numOfFeats; j++)
spPointDestroy(resPoints[j]);
free(resPoints);
}
}
// spLoggerPrintInfo(EXIT_MSG);
// Creation of point array with features of all images.
totalResPoints = createTotalFeatArray(config, numOfImg, dim,&sizeOfTotalFeat); //CHECKK HERE/ LIRON GET CALM
if(totalResPoints==NULL){
spLoggerPrintError(FEAT_ARRAY_INIT_FAIL_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Creation of KDArray with all point features - with image attribute
kdarray = KdArrayInit(totalResPoints, sizeOfTotalFeat);
//Destroying the SPPoint Total features Array.
for (int i = 0; i < KDArrayGetSize(kdarray); i++)
spPointDestroy(totalResPoints[i]);
free(totalResPoints);
//Checking Split Method from configuration file
spConfigGetKDSplitMethod(&method, config);
//Initializing KDTree DB
root = kdTreeInit(kdarray, method, -1);
//Search By Query
puts(ENTER_IMG_PATH);
fflush(NULL);
scanf("%s", imagePath);
fflush(NULL);
//Declaring countHits for count similar features per image.
struct featHits* countHits = (featHits*) malloc(sizeof(featHits) * numOfImg);
if(countHits==NULL){
spLoggerPrintError(FAIL_ALOC_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Request Query Image Path until 'exit' enter
while (strcmp(imagePath, "<>") != 0) {
spLoggerPrintInfo(LOG_CURRENT_IMAGE_MSG);
spLoggerPrintInfo(imagePath);
for (int i = 0; i < numOfImg; i++) {
countHits[i].hits = 0;
countHits[i].index = i;
}
//Extracting Image features for query image into SPPoint Array.
resPoints = imageProc.getImageFeatures(imagePath, BAD_INDEX,numOfFeats);
if(resPoints==NULL){
spLoggerPrintError(FAIL_ALOC_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
//Searching for nearest neighbors of each feature
for (int i = 0; i < *numOfFeats; i++) {
//Creating new BPQueue MaxSize spKNN
SPBPQueue bpq = spBPQueueCreate(spKNN);
if(bpq==NULL){
spLoggerPrintError(BPQUEUE_FAILURE,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
KD_TREE_MSG treeMsg = kNearestNeighbors(bpq, root, resPoints[i]);
if(treeMsg!=KD_TREE_SUCCESS){
spLoggerPrintError(KD_TREE_FAIL_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
for (int j = 0; j < spKNN; j++) {
SP_BPQUEUE_MSG bpqMsg;
int index = spBPQueuePeekIndex(bpq);
countHits[index].hits++;
bpqMsg = spBPQueueDequeue(bpq);
if(bpqMsg!=SP_BPQUEUE_SUCCESS){
spLoggerPrintError(BPQUEUE_FAILURE,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
}
spBPQueueDestroy(bpq);
}
//Sorting and getting the K best hits
qsort((void*) countHits, numOfImg, sizeof(featHits), hitsComp);
//Checking Show Image State
if (spConfigMinimalGui(config, &msg)) {
for (int i = 0; i < numSimilarImg; i++) {
msg = spConfigGetImagePath(imagePath, config,
countHits[i].index);
if(msg!=SP_CONFIG_SUCCESS){
spLoggerPrintError(CONFIG_FAIL_MSG,__FILE__,__func__,__LINE__);
return 1; //exit(1)
}
imageProc.showImage(imagePath);
}
} else {
printf("Best candidates for - %s - are:\n", imagePath);
for (int i = 0; i < numSimilarImg; i++){
spConfigGetImagePath(imagePath,config,countHits[i].index);
printf("%s\n",imagePath);
}
}
spLoggerPrintInfo("Wating for another query image path");
puts(ENTER_IMG_PATH);
fflush(NULL);
scanf("%s", imagePath);
}
spLoggerPrintInfo(EXIT_MSG);
free(loggerFile);
spLoggerDestroy();
free(countHits);
KDTreeDestroy(root);
free(imagePath);
free(numOfFeats);
return 0;
}
int main(int argc,char* argv[]){
SPConfig config;
char configpath[1024] = ""; // asked moab can assume that not more than 1024 char
if(argc > 1){ // if inserted command line arguments
if ((argc == 3)&&(!(strcmp(argv[1],"-c")))){ // checks if a path was inserted in the correct format
strcpy(configpath,argv[2]);
}else{
printf("Invalid command line : use -c <config_filename>\n");
return 0;
}
}
if (!strcmp(configpath,"")){ //if no path was inserted
strcpy(configpath,"spcbir.config");
}
SP_CONFIG_MSG msg;
config = spConfigCreate(configpath,&msg);
if (config == NULL){ // if there was an error
if (msg == SP_CONFIG_CANNOT_OPEN_FILE){
if(!strcmp(configpath,"spcbir.config")){
printf("The default configuration file spcbir.config couldn’t be open\n");
return 0;
}else{
printf("The configuration file %s couldn’t be open\n",configpath);
return 0;
}
}
}
// finished creating configuration file
spLoggerCreate(spConfigGetLoggerFilename(config),spConfigGetLoggerLevel(config));
//finished creating SPLogger
ImageProc proc = ImageProc(config);
spLoggerPrintInfo("Creating imageproc success");
// finished creating image processing instance
int i,j,k;
int n=0;
int numofimages = spConfigGetNumOfImages(config,&msg);
int* tempdir = (int*)malloc(sizeof(int)*numofimages);
if(tempdir == NULL){spLoggerPrintError("tempdir is null!",__FILE__,__func__,__LINE__);}
else{spLoggerPrintDebug("tempdir allocation is success!!",__FILE__,__func__,__LINE__);}
char temppath[1024];
int tempnumOfFeatsextracted;
SPPoint** directory = (SPPoint**)malloc(sizeof(SPPoint*)*numofimages); // allocating matrix of feats per image
if(directory == NULL){spLoggerPrintError("directory is null!",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("directory allocation is success!!",__FILE__,__func__,__LINE__);}
SPPoint* finaldir;
if (spConfigGetExtractionMode(config)){ // if we are in extraction mode
FILE* fw;
for (i = 0; i < numofimages; i++) { //for each image in image directory
msg = spConfigGetImagePath (temppath,config,i);
if(msg != SP_CONFIG_SUCCESS){spLoggerPrintError(spConfigMsgToString(msg),__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug(spConfigMsgToString(msg),__FILE__,__func__,__LINE__);}
directory[i] = proc.getImageFeatures(temppath,i,&tempnumOfFeatsextracted);
if(directory[i]==NULL){spLoggerPrintError("error extracting image features",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("extracted features from image successfully",__FILE__,__func__,__LINE__);}
n+=tempnumOfFeatsextracted;
tempdir[i] = tempnumOfFeatsextracted;
msg = spConfigGetImagePathfeats(temppath,config,i); //get the file to write to
if(msg != SP_CONFIG_SUCCESS){spLoggerPrintError(spConfigMsgToString(msg),__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("successfully extracted image path to write to",__FILE__,__func__,__LINE__);}
fw = fopen(temppath,"w");//open file for writing
writefeats(fw,directory[i],tempnumOfFeatsextracted);
}
fclose(fw);
finaldir = (SPPoint*)malloc(sizeof(SPPoint)*n); //making final dir
if (finaldir == NULL){spLoggerPrintError("error initializing finaldir in extraction mode",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("initialized finaldir successfully",__FILE__,__func__,__LINE__);}
k=0;
for (i = 0; i < numofimages; ++i){
for (j = 0; j < tempdir[i]; ++j) {
finaldir[k] = spPointCopy(directory[i][j]);
k++;
}
}
DirectoryDestroy(directory,tempdir,numofimages);
}
else{ // non - extraction mode
FILE* fr;
for (i = 0; i < numofimages; i++){
msg = spConfigGetImagePathfeats(temppath,config,i); //get path to feats file to read from
if (msg !=SP_CONFIG_SUCCESS){spLoggerPrintError("error extracting feats path from config",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("successfully extracted feats path from config",__FILE__,__func__,__LINE__);}
fr = fopen(temppath,"r");
if(fr ==NULL){spLoggerPrintError("error opening .feats file to extract",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("opend .feats file successfully",__FILE__,__func__,__LINE__);}
directory[i] = getfeats(fr,(tempdir+i));
n+=tempdir[i];
}
fclose(fr);
finaldir = (SPPoint*)malloc(sizeof(SPPoint)*n); //making final dir
if (finaldir == NULL){spLoggerPrintError("error initializing finaldir in extraction mode",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("initialized finaldir successfully",__FILE__,__func__,__LINE__);}
k=0;
for (i = 0; i < numofimages; ++i){
for (j = 0; j < tempdir[i]; ++j) {
finaldir[k] = spPointCopy(directory[i][j]);
k++;
}
}
DirectoryDestroy(directory,tempdir,numofimages);
}
//after extraction / non - extraction and we have our finaldir containing all the feats(SPPoints)!
spLoggerPrintInfo("success after extraction / non - extraction phase");
bool out = false;
int numofsimilarimages = spConfigGetNumSimilarImages(config);
KDTreeNode* head = InitTree(finaldir,n,config);
if(head ==NULL){spLoggerPrintError("failure building tree",__FILE__,__func__,__LINE__);return 0;}
else{spLoggerPrintDebug("tree build complete successfully",__FILE__,__func__,__LINE__);}
/*initialization of KDTree & free of finaldir complexity: O(d X nlogn)*/
while(!out){
printf("Please enter an image path:\n");
fflush(stdout);
char quarypath[1024]; //= (char*)malloc(sizeof(char)*1024); //moab said can assume quary path is at most 1024
scanf("%s",quarypath);
if(!strcmp(quarypath,"<>")){ //if chose to exit the program
printf("Exiting…\n");
out = true;
break;
}
int* hits = (int*)malloc(sizeof(int)*numofimages);
for(i=0;i<numofimages;i++){//initialize to -1 hits per image
hits[i] = -1;
}
//an array to keep track of how many times an image feature was selected to be k-nearest feature
int* winners = (int*)malloc(sizeof(int)*numofsimilarimages);
//an array contains indexes of winners ordered by numbered of hits example: winner[0] - the index of the most closest image
SPPoint* quaryfeatures = proc.getImageFeatures(quarypath,numofimages,&tempnumOfFeatsextracted);
SPBPQueue tempbpq;
for (i = 0; i < tempnumOfFeatsextracted; ++i) { //count hits per image
tempbpq = KDTreeSearch(head,quaryfeatures[i],numofsimilarimages);
while(!spBPQueueIsEmpty(tempbpq)){
hits[spListElementGetIndex(spBPQueuePeek(tempbpq))]++;
spBPQueueDequeue(tempbpq);
}
spBPQueueDestroy(tempbpq);
}
calculatewinners(winners,hits,numofimages,numofsimilarimages);
SPPointArrayDestroy(quaryfeatures,tempnumOfFeatsextracted);
//showing results
spLoggerPrintInfo("showing results");
if(spConfigMinimalGui(config,&msg)){ // if we are in minimal-GUI mode
for (i = 0; i <numofsimilarimages ; ++i) {
spConfigGetImagePath(temppath,config,winners[i]);
proc.showImage(temppath);
}
free(hits);
free(winners);
}
else{ // not in minimal-GUI mode
printf("Best candidates for - %s - are:\n",quarypath);
for (i = 0; i < numofsimilarimages; ++i) {
spConfigGetImagePath(temppath,config,winners[i]);
printf("%s\n",temppath);
}
free(hits);
free(winners);
}
}
KDTreeDestroy(head);
spConfigDestroy(config);
free(tempdir);
spLoggerDestroy();
spLoggerPrintInfo("finished free of all memory successfully");
return 1;
}
int main(int argc, char** argv)
{
const char* image_name = argc > 1 ? argv[1] : "maze6.png";
// Mat triangle = imread(image_name, CV_LOAD_IMAGE_GRAYSCALE);
// VideoCapture cap;
// cout << atan2(1, 0) << endl;
// cout << atan2(1, 0) << endl;
// if (!cap.open(0))
// {
// cout << "Camera not available" << endl;
// return 0;
// }
// Mat triangle;
// Point position;
// cap >> triangle;
// Vec2f angle;
// if (processing.get_triangle(triangle, angle, position))
// {
// cout << angle << " " << position << endl;
// }
// else
// {
// cout << "No triangle found" << endl;
// }
// // line(color_maze, vehicle, vehicle + Point((next_step_vector * 10)[0], (next_step_vector * 10)[1]), 2);
// line(triangle, position, position + Point((angle * 100)[0], (angle * 100)[1]), 2);
// imwrite("triangle.png", triangle);
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = my_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
control.stop();
freopen("values.txt", "r", stdin);
int c1, c2, c3;
cin >> c1 >> c2 >> c3;
Mat color_maze = imread(image_name, CV_LOAD_IMAGE_COLOR),
position_maze;
// if (!cap.open(0))
// {
// cout << "Camera not available" << endl;
// return 0;
// }
// Mat color_maze, position_maze;
// cap >> color_maze;
Mat thresh_maze(color_maze.rows, color_maze.cols, CV_8UC1);
cvtColor(color_maze, thresh_maze, COLOR_BGR2GRAY);
Vec2f vehicle_angle(0.0f, 1.0f);
const double vehicle_speed = 5.0;
const double rotation_speed = 0.3;
double new_v_y, new_v_x;
Point2f vehicle, end;
preprocess(thresh_maze, color_maze, 500);
// processing.get_point(color_maze, end, c3, 15, c1, c2);
end = Point2f(20, 20);
// DEMO
if (!processing.get_triangle(color_maze, vehicle_angle, vehicle))
{
cout << "NO TRIANGLE" << endl;
vehicle = Point2f(300, 300);
imwrite("webcam.png", color_maze);
}
else
{
cout << "position " << vehicle << endl;
}
circle(color_maze, end, 10, Scalar(0, 0, 255));
cout << "Destination: " << end << " start: " << vehicle << endl;
imwrite("webcam.png", color_maze);
cout << "Performing search" << endl;
solver.depth_first_search(color_maze, thresh_maze, end.x, end.y);
cout << "Drawing path" << endl;
solver.draw_path(color_maze, vehicle.x, vehicle.y);
imwrite("path.png", color_maze);
cout << "Starting cycle" << endl;
for (int step = 0; step < 500; step++)
{
Mat current_frame = color_maze.clone();
// cap >> position_maze;
// if (!processing.get_triangle(position_maze, vehicle_angle, vehicle))
// {
// cout << "Could not find triangle" << endl;
// }
Vec2f next_step_vector = solver.next_step(vehicle.x, vehicle.y, 5);
if (next_step_vector == Vec2f())
{
cout << "Found exit!" << endl;
break;
}
// Obtain the angle between the two vectors
// double angle =
// atan2(next_step_vector[1], next_step_vector[0]) -
// atan2(vehicle_angle[1], vehicle_angle[0]);
line(current_frame, vehicle, vehicle + Point2f((next_step_vector * 10)[0], (next_step_vector * 10)[1]), Scalar(0, 0, 255), 2);
line(current_frame, vehicle, vehicle + Point2f((vehicle_angle * 20)[0], (vehicle_angle * 20)[1]), Scalar(255, 0, 0), 2);
circle(current_frame, vehicle, 3, Scalar(0, 255, 255));
// color_maze.at<Vec3b>(next_destination.second, next_destination.first) = Vec3b(255, 255, 0);
double direction = next_step_vector[0] * vehicle_angle[1] - vehicle_angle[0] * next_step_vector[1];
cout << vehicle << " " << next_step_vector << " " << vehicle_angle << " " << direction << " " << step << endl;
if (direction > 0.2)
{
control.left();
cout << "left" << endl;
new_v_x = vehicle_angle[0] * cos(-rotation_speed) - vehicle_angle[1] * sin(-rotation_speed);
new_v_y = vehicle_angle[0] * sin(-rotation_speed) + vehicle_angle[1] * cos(-rotation_speed);
vehicle_angle[0] = new_v_x;
vehicle_angle[1] = new_v_y;
}
else if (direction < -0.2)
{
control.right();
cout << "right" << endl;
new_v_x = vehicle_angle[0] * cos(rotation_speed) - vehicle_angle[1] * sin(rotation_speed);
new_v_y = vehicle_angle[0] * sin(rotation_speed) + vehicle_angle[1] * cos(rotation_speed);
vehicle_angle[0] = new_v_x;
vehicle_angle[1] = new_v_y;
}
else
{
control.forward();
cout << "forward " << vehicle << " moving by " << vehicle_speed * vehicle_angle[0]
<< " , " << vehicle_speed * vehicle_angle[1] << endl;
vehicle.x += vehicle_speed * vehicle_angle[0];
vehicle.y += vehicle_speed * vehicle_angle[1];
vehicle.x = std::min(std::max(vehicle.x, 0.0f), color_maze.cols - 1.0f);
vehicle.y = std::min(std::max(vehicle.y, 0.0f), color_maze.rows - 1.0f);
}
if (vehicle.y >= 0 && vehicle.y < color_maze.rows &&
vehicle.x >= 0 && vehicle.x < color_maze.cols) {
color_maze.at<Vec3b>(vehicle.y, vehicle.x) = Vec3b(255, 0, 255);
}
ostringstream ss;
ss << "frames/" << setfill('0') << setw(3) << step << ".png";
imwrite(ss.str(), current_frame);
// delay(50);
}
cout << "Finished!" << endl;
control.stop();
// imshow("sol", color_maze);
imwrite("sol.png", color_maze);
cvWaitKey();
return 1;
}
/* new main function that tests using the image processing object
* with the multithreadded updater
*/
int main(int argc, char* argv[])
{
ImageProc improc;
if(improc.init() == -1)
{
cout << "could not init improc" << endl;
return -1;
}
ConfigReader settings("../data/config.txt");
if(settings.has("threshold_hue_low"))
{
improc.tHue.low = std::stoi(settings.get("threshold_hue_low"));
}
if(settings.has("threshold_hue_high"))
{
improc.tHue.high = std::stoi(settings.get("threshold_hue_high"));
}
if(settings.has("threshold_sat_low"))
{
improc.tSat.low = std::stoi(settings.get("threshold_sat_low"));
}
if(settings.has("threshold_sat_high"))
{
improc.tSat.high = std::stoi(settings.get("threshold_sat_high"));
}
if(settings.has("threshold_val_low"))
{
improc.tVal.low = std::stoi(settings.get("threshold_val_low"));
}
if(settings.has("threshold_val_high"))
{
improc.tVal.high = std::stoi(settings.get("threshold_val_high"));
}
namedWindow("ThresholdValues", CV_WINDOW_NORMAL);
cvCreateTrackbar("LowH", "ThresholdValues", &improc.tHue.low, 179);
cvCreateTrackbar("HighH", "ThresholdValues", &improc.tHue.high, 179);
cvCreateTrackbar("LowS", "ThresholdValues", &improc.tSat.low, 255);
cvCreateTrackbar("HighS", "ThresholdValues", &improc.tSat.high, 255);
cvCreateTrackbar("LowV", "ThresholdValues", &improc.tVal.low, 255);
cvCreateTrackbar("HighV", "ThresholdValues", &improc.tVal.high, 255);
namedWindow("CameraImage", CV_WINDOW_NORMAL);
namedWindow("FilterImage", CV_WINDOW_NORMAL);
zwlib::ContinuousUpdater<ImageData> cu(&improc,std::chrono::milliseconds(50));
cu.start();
while(!cu.ready()) {}
while(waitKey(1) == -1)
{
ImageData data = cu.get();
Mat image = data.image;
Mat threshImage = data.filtered_image;
Scalar lineColor(0,255,0);
int lineWidth = 2;
double area = data.object.width * data.object.height;
double meanLength = sqrt(area);
circle(
image,
Point(data.object.x+data.object.width/2, data.object.y+data.object.height/2),
meanLength/2,
lineColor, lineWidth
);
line(
image,
Point(data.object.x, data.object.y+data.object.height/2),
Point(data.object.x+data.object.width, data.object.y+data.object.height/2),
lineColor, lineWidth
);
line(
image,
Point(data.object.x+data.object.width/2, data.object.y),
Point(data.object.x+data.object.width/2, data.object.y+data.object.height),
lineColor, lineWidth
);
imshow("CameraImage", image);
imshow("FilterImage", threshImage);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
return 0;
}
SPPoint *extractAllFeatures(SPConfig config, int *numberOfFeatures, SP_DATABASE_CREATION_MSG *msg) {
char *imagePath = NULL, *featuresPath = NULL;
SPPoint *allFeatures = NULL;
SP_CONFIG_MSG resultMSG;
int numOfImages = spConfigGetNumOfImages(config, &resultMSG);
if (resultMSG != SP_CONFIG_SUCCESS) {
*msg = SP_DATABASE_CREATION_CONFIG_ERROR;
return NULL;
}
ImageProc ip = ImageProc(config);
allFeatures = (SPPoint *) malloc(1 * sizeof(SPPoint));
imagePath = (char *) malloc (MAX_PATH_LENGTH * sizeof(char));
featuresPath = (char *) malloc (MAX_PATH_LENGTH * sizeof(char));
if (allFeatures == NULL || imagePath == NULL || featuresPath == NULL) {
destroyVariables(allFeatures, 0, imagePath, featuresPath);
*msg = SP_DATABASE_CREATION_ALLOC_FAIL;
return NULL;
}
int numOfFeaturesExtracted;
int totalFeaturesCount = 0;
for (int imageIndex = 0; imageIndex < numOfImages; imageIndex++) {
if (spConfigGetImagePath(imagePath, config, imageIndex) != SP_CONFIG_SUCCESS ||
spConfigGetImageFeaturesPath(featuresPath, config, imageIndex) != SP_CONFIG_SUCCESS) {
destroyVariables(allFeatures, totalFeaturesCount, imagePath, featuresPath);
*msg = SP_DATABASE_CREATION_CONFIG_ERROR;
return NULL;
}
SPPoint *features = ip.getImageFeatures(imagePath, imageIndex, &numOfFeaturesExtracted);
if (features == NULL || numOfFeaturesExtracted <= 0) {
destroyVariables(allFeatures, totalFeaturesCount, imagePath, featuresPath);
*msg = SP_DATABASE_CREATION_FEATURES_EXTRACTION_ERROR;
return NULL;
}
totalFeaturesCount += numOfFeaturesExtracted;
SP_DATABASE_CREATION_MSG creationMSG = writeFeatures(featuresPath, features, numOfFeaturesExtracted);
if (creationMSG != SP_DATABASE_CREATION_SUCCESS) {
destroyVariables(allFeatures, totalFeaturesCount, imagePath, featuresPath);
freePointsArray(features, numOfFeaturesExtracted);
*msg = creationMSG;
return NULL;
}
allFeatures = (SPPoint *) realloc(allFeatures, totalFeaturesCount * sizeof(SPPoint));
if (allFeatures == NULL) {
destroyVariables(allFeatures, totalFeaturesCount, imagePath, featuresPath);
freePointsArray(features, numOfFeaturesExtracted);
*msg = SP_DATABASE_CREATION_ALLOC_FAIL;
return NULL;
}
for (int i = 0; i < numOfFeaturesExtracted; i++) {
allFeatures[totalFeaturesCount - numOfFeaturesExtracted + i] = features[i];
}
}
free(imagePath);
free(featuresPath);
*numberOfFeatures = totalFeaturesCount;
*msg = SP_DATABASE_CREATION_SUCCESS;
return allFeatures;
}
SDL_Surface* CnCMap::getMiniMap(Uint8 pixsize) {
static ImageProc ip;
SDL_Rect pos = {0, 0, pixsize, pixsize};
SDL_Surface *cminitile;
if (pixsize == 0) {
// Argh
logger->error("CnCMap::getMiniMap: pixsize is zero, resetting to one\n");
pixsize = 1;
}
if(minimap != NULL) {
if (minimap->w == width*pixsize) {
return minimap;
} else {
// Each minimap surface is about 250k, so caching a lot of zooms
// would be somewhat expensive. Could make how much memory to set aside
// for this customizable so people with half a gig of RAM can see some
// usage :-)
// For now, just keep the previous one.
SDL_FreeSurface(oldmmap);
oldmmap = minimap;
minimap = NULL;
}
}
if (oldmmap != NULL && (oldmmap->w == pixsize*width)) {
minimap = oldmmap;
oldmmap = NULL;
return minimap;
}
minimap = SDL_CreateRGBSurface (SDL_SWSURFACE, width*pixsize,height*pixsize, 16,
0xff, 0xff, 0xff, 0);
SDL_Surface *maptileOne = getMapTile(0);
minimap->format->Rmask = maptileOne->format->Rmask;
minimap->format->Gmask = maptileOne->format->Gmask;
minimap->format->Bmask = maptileOne->format->Bmask;
minimap->format->Amask = maptileOne->format->Amask;
if( maptileOne->format->palette != NULL ) {
SDL_SetColors(minimap, maptileOne->format->palette->colors, 0,
maptileOne->format->palette->ncolors);
}
int lineCounter = 0;
for(Uint32 i = 0; i < (Uint32) width*height; i++, pos.x += pixsize,
lineCounter++) {
if(lineCounter == width) {
pos.y += pixsize;
pos.x = 0;
lineCounter = 0;
}
cminitile = ip.minimapScale(getMapTile(i), pixsize);
SDL_BlitSurface(cminitile, NULL, minimap, &pos);
SDL_FreeSurface(cminitile);
}
/* Now fill in clipping details for renderer and UI.
* To make things easier, ensure that the geometry is divisable by the
* specified width and height.
*/
Uint16 tx = min(miniclip.sidew, (Uint16)minimap->w);
Uint16 ty = min(tx, (Uint16)minimap->h);
// w == width in pixels of the minimap
miniclip.w = pixsize*(int)floor((double)tx/(double)pixsize);
miniclip.h = pixsize*(int)floor((double)ty/(double)pixsize);
// x == offset in pixels from the top-left hand corner of the sidebar under
// the tab.
miniclip.x = abs(miniclip.sidew-miniclip.w)/2+20;
miniclip.y = abs(miniclip.sidew-miniclip.h)/2+5;
// Tilew == number of tiles visible in minimap horizontally
miniclip.tilew = miniclip.w/pixsize;
miniclip.tileh = miniclip.h/pixsize;
// pixsize == number of pixels wide and high a minimap tile is
miniclip.pixsize = pixsize;
return minimap;
}
