// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description : This is the entry point of the program.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Image of AR.Drone's camera
IplImage *image = ardrone.getImage();
// Read intrincis camera parameters
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_READ);
CvMat *intrinsic = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "intrinsic"));
CvMat *distortion = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "distortion"));
// Initialize undistortion maps
CvMat *mapx = cvCreateMat(image->height, image->width, CV_32FC1);
CvMat *mapy = cvCreateMat(image->height, image->width, CV_32FC1);
cvInitUndistortMap(intrinsic, distortion, mapx, mapy);
// Main loop
while (1) {
// Key input
int key = cvWaitKey(1);
if (key == 0x1b) break;
// Update
if (!ardrone.update()) break;
// Get an image
image = ardrone.getImage();
// Remap the image
cvRemap(image, image, mapx, mapy);
// Display the image
cvShowImage("camera", image);
}
// Release the matrices
cvReleaseMat(&mapx);
cvReleaseMat(&mapy);
cvReleaseFileStorage(&fs);
// See you
ardrone.close();
return 0;
}
bool ObjectDefinition::Load(const char* filename)
{
do{
CvFileStorage* storage = cvOpenFileStorage(filename, 0, CV_STORAGE_READ);
m_keypoints = (CvSeq*)cvRead(storage, cvGetFileNodeByName(storage, 0, "keypoints"));
m_descriptor = (CvSeq*)cvRead(storage, cvGetFileNodeByName(storage, 0, "descriptor"));
cvReadRawData(storage, cvGetFileNodeByName(storage, 0, "corners"), m_corners, "ii");
cvReleaseFileStorage(&storage);
m_filename = filename;
return true;
}while(false);
return false;
}
int
main (int argc, char **argv)
{
char filename[] = "save_cv.xml"; // file name
int i,j,k;
CvFileStorage *cvfs;
CvFileNode *node, *fn;
CvSeq *s;
int total;
// (1)memory space for loading data
int a;
float b;
CvMat** mat = (CvMat**)cvAlloc(3*sizeof(CvMat*));
// (2)open file storage
cvfs = cvOpenFileStorage(filename, NULL, CV_STORAGE_READ);
// (3)read data from file storage
node = cvGetFileNodeByName(cvfs, NULL, ""); // Get Top Node
a = cvReadIntByName(cvfs, node, "a", 0);
b = cvReadRealByName(cvfs, node, "b", 0);
fn = cvGetFileNodeByName(cvfs,node,"mat_array");
s = fn->data.seq;
total = s->total;
for(i=0;i<total;i++){
mat[i] = (CvMat*)cvRead(cvfs,(CvFileNode*)cvGetSeqElem(s,i), NULL);
}
// (4)close file storage
cvReleaseFileStorage(&cvfs);
// (5)print loaded data
printf("a:%d\n", a);
printf("b:%f\n", b);
for(i=0; i<3; i++){
printf("mat%d:\n",i);
for(j=0;j<mat[i]->rows;j++){
for(k=0;k<mat[i]->cols;k++){
printf("%f,",cvmGet(mat[i],j,k));
}
printf("\n");
}
}
// release mat
for(i=0; i<3; i++){
cvReleaseMat(mat+i);
}
cvFree(mat);
return 0;
}
bool CvMatrix::read( CvFileStorage* fs, const char* seqname, int idx )
{
void* obj = 0;
CvMat* m = 0;
CvFileNode* seqnode = seqname ?
cvGetFileNodeByName( fs, 0, seqname ) : cvGetRootFileNode(fs,0);
if( seqnode && CV_NODE_IS_SEQ(seqnode->tag) )
obj = cvRead( fs, (CvFileNode*)cvGetSeqElem( seqnode->data.seq, idx ));
m = icvRetrieveMatrix(obj);
set( m, false );
return m != 0;
}
bool CvImage::read( CvFileStorage* fs, const char* seqname, int idx )
{
void* obj = 0;
IplImage* img = 0;
CvFileNode* seqnode = seqname ?
cvGetFileNodeByName( fs, 0, seqname ) : cvGetRootFileNode(fs,0);
if( seqnode && CV_NODE_IS_SEQ(seqnode->tag) )
obj = cvRead( fs, (CvFileNode*)cvGetSeqElem( seqnode->data.seq, idx ));
img = icvRetrieveImage(obj);
attach( img );
return img != 0;
}
int
main (int argc, char *argv[])
{
// IMAGE_NUM, PAT_ROW, PAT_COL,PAT_SIZE, ALL_POINTS, CHESS_SIZE
/*
if (argc < 6)
{
std::cout<< "ERROR : augment is incorrect" << std::endl;
return -1;
}
*/
//int PAT_ROW = atoi(argv[3]);
//int PAT_COL = atoi(argv[4]);
//int CHESS_SIZE = atoi(argv[5]);
//int PAT_SIZE = PAT_ROW*PAT_COL;
char* NAME_IMG_IN = argv[1];
//char* NAME_XML_OUT = argv[2];
int i,j;
int corner_count, found;
IplImage *src_img;
CvSize pattern_size = cvSize(PAT_COL, PAT_ROW);
CvMat image_points;
CvMat object_points;
CvMat *intrinsic, *distortion;
CvMat *rotation = cvCreateMat(1, 3, CV_32FC1);
CvMat *rotationConv = cvCreateMat(3, 3, CV_32FC1);
CvMat *translation = cvCreateMat(1, 3, CV_32FC1);
CvPoint3D32f objects[PAT_SIZE];
CvFileStorage *fs;
CvFileNode *param;
CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * PAT_SIZE);
// (1)�����оݤȤʤ������ɤ߹���
if ( ( src_img = cvLoadImage(NAME_IMG_IN, CV_LOAD_IMAGE_COLOR) ) == 0)
//if (argc < 2 || (src_img = cvLoadImage (argv[1], CV_LOAD_IMAGE_COLOR)) == 0)
{
std::cout<< "ERROR : input image is not exist or augment is incorrect" << std::endl;
return -1;
}
// 3�������ֺ�ɸ������
for (i = 0; i < PAT_ROW; i++) {
for (j = 0; j < PAT_COL; j++) {
objects[i * PAT_COL + j].x = i * CHESS_SIZE;
objects[i * PAT_COL + j].y = j * CHESS_SIZE;
objects[i * PAT_COL + j].z = 0.0;
}
}
cvInitMatHeader(&object_points, PAT_SIZE, 3, CV_32FC1, objects);
// �������ܡ��ɡʥ����֥졼�����ѥ�����ˤΥ����ʡ�����
int found_num = 0;
// cvNamedWindow("Calibration", CV_WINDOW_AUTOSIZE);
found = cvFindChessboardCorners(src_img, pattern_size, &corners[0], &corner_count);
fprintf(stderr, "corner:%02d...\n", corner_count);
if (found) {
fprintf(stderr, "ok\n");
} else {
fprintf(stderr, "fail\n");
}
// (4)�����ʡ����֤֥ԥ��������٤˽���������
IplImage *src_gray = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 1);
cvCvtColor (src_img, src_gray, CV_BGR2GRAY);
cvFindCornerSubPix (src_gray, &corners[0], corner_count,
cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvDrawChessboardCorners (src_img, pattern_size, &corners[0], corner_count, found);
// cvShowImage ("Calibration", src_img);
// cvWaitKey (0);
// cvDestroyWindow("Calibration");
cvShowImage ("Calibration", src_img);
cvInitMatHeader(&image_points, PAT_SIZE, 1, CV_32FC2, corners);
// (2)�ѥ����ե�������ɤ߹���
fs = cvOpenFileStorage ("xml/rgb.xml", 0, CV_STORAGE_READ);
param = cvGetFileNodeByName (fs, NULL, "intrinsic");
intrinsic = (CvMat *) cvRead (fs, param);
param = cvGetFileNodeByName (fs, NULL, "distortion");
distortion = (CvMat *) cvRead (fs, param);
cvReleaseFileStorage (&fs);
// (3) �����ѥ����ο���
CvMat sub_image_points, sub_object_points;
int base = 0;
cvGetRows(&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
cvGetRows(&object_points, &sub_object_points, base * PAT_SIZE, (base + 1)* PAT_SIZE);
cvFindExtrinsicCameraParams2(&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);
int ret = cvRodrigues2(rotation, rotationConv);
// int cols = sub_object_points.rows;
// printf("cols = %d\n", cols);
// printf("%f\n",sub_object_points.data.fl[0]);
// mm -> m
for (i = 0; i < translation->cols; i++) { translation->data.fl[i] = translation->data.fl[i] / 1000;}
// (4)XML�ե�����ؤνФ�
//fs = cvOpenFileStorage(argv[2], 0, CV_STORAGE_WRITE);
fs = cvOpenFileStorage(NAME_XML_OUT, 0, CV_STORAGE_WRITE);
cvWrite(fs, "rotation", rotationConv);
cvWrite(fs, "translation", translation);
cvReleaseFileStorage(&fs);
/////////////////////////////////////////////////
// write out py
if(1)
{
cv::Mat ttt(translation);
cv::Mat rrr(rotationConv);
char data2Write[1024];
char textFileName[256];
sprintf( textFileName , "cbCoord/cbOneShot.py");
std::ofstream outPy(textFileName);
outPy << "import sys" <<std::endl;
outPy << "sys.path.append('../')" <<std::endl;
outPy << "from numpy import *" <<std::endl;
outPy << "from numpy.linalg import svd" <<std::endl;
outPy << "from numpy.linalg import inv" <<std::endl;
outPy << "from chessboard_points import *"<<std::endl;
outPy << "sys.path.append('../geo')" <<std::endl;
outPy << "from geo import *" <<std::endl;
/*
///////////////////////////////////////////////////////////////////////////////////
// out translation and rotation as xyzabc list
outPy << "xyzabc = []" <<std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out translation and rotation as xyzabc list
///////////////////////////////////////////////////////////////////////////////////
*/
///////////////////////////////////////////////////////////////////////////////////
// out translation
outPy << "ttt = []" <<std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out translation
//////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
// out rotation
outPy << "rrr = []" <<std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(0), rrr.at<float>(1), rrr.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(3), rrr.at<float>(4), rrr.at<float>(5) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(6), rrr.at<float>(7), rrr.at<float>(8) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out rotation
//////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
outPy<< "_T = FRAME( vec=ttt, mat=rrr )" << std::endl;
/////////////////////////////////////////////////////////////////
}
// write out py
/////////////////////////////////////////////////
std::cout<< "press any key..."<< std::endl;
cvWaitKey (0);
cvDestroyWindow("Calibration");
cvReleaseImage(&src_img);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
return 0;
}
/*
* Load a Protocol From yaml File
*
*/
Protocol* LoadProtocolFromFile(const char* filename){
Protocol* myP=CreateProtocolObject();
LoadProtocolWithFilename(filename,myP);
CvFileStorage* fs=cvOpenFileStorage(myP->Filename,0,CV_STORAGE_READ);
printf("Opened File: %s for reading\n", myP->Filename);
/** Point to Protocol Object **/
CvFileNode* protonode=cvGetFileNodeByName(fs,NULL,"Protocol");
/** Load in Description **/
CvFileNode* node=cvGetFileNodeByName(fs,protonode,"Description");
myP->Description=copyString(cvReadString(node,NULL));
printf("Loading in Protocol, Description:\n %s\n",myP->Description);
/** Load in Grid Size **/
node=cvGetFileNodeByName(fs,protonode,"GridSize");
int height=cvReadIntByName(fs,node,"height",-1);
int width=cvReadIntByName(fs,node,"width",-1);
printf(" width =%d\n",width );
printf(" height=%d\n",height);
if (height>0 && width>0){
myP->GridSize=cvSize(width,height);
}
/** Create the Steps Object and Load it into the Protocol **/
myP->Steps=CreateStepsObject(myP->memory);
/** Point to the Steps node in the YAML file **/
node=cvGetFileNodeByName(fs,protonode,"Steps");
/** Create a local object that contains the information of the steps **/
CvSeq* stepSeq=node->data.seq;
int numsteps=stepSeq->total;
printf("numsteps=%d\n",numsteps);
CvSeqReader StepReader;
cvStartReadSeq( stepSeq, &StepReader, 0 );
/** Let's loop through all of the steps **/
for (int i= 0; i< numsteps; ++i) {
/**Create Illumination Montage Object **/
CvSeq* montage=CreateIlluminationMontage(myP->memory);
/** Find the node of the current image montage (step) **/
CvFileNode* montageNode = (CvFileNode*)StepReader.ptr;
CvSeq* montageSeq=montageNode->data.seq;
int numPolygonsInMontage=montageSeq->total;
// printf("Step %d: %d polygon(s) found\n",i,numPolygonsInMontage);
CvSeqReader MontageReader;
cvStartReadSeq( montageSeq, &MontageReader, 0 );
/** Loop through all of the polygons **/
for (int k = 0; k < numPolygonsInMontage; ++k) {
/** Load the CvSeq Polygon Objects and push them onto the montage **/
CvFileNode* polygonNode = (CvFileNode*)MontageReader.ptr;
CvSeq* polygonPts =(CvSeq*) cvRead(fs,polygonNode); // <---- Andy come back here.
printf("\tStep %d, Polygon %d: %d points found.\n",i,k,polygonPts->total);
/**
* Now we have the points for our polygon so we need to load
* those points into a polygon object
*/
WormPolygon* polygon= CreateWormPolygonFromSeq(myP->memory,myP->GridSize,polygonPts);
//printf("\t\t %d points copied\n",polygon->Points->total);
/** Add the polygon to the montage **/
cvSeqPush(montage,&polygon);
//printf("\t\t Current montage now has %d polygons\n",montage->total);
/** Move to the next polygon **/
CV_NEXT_SEQ_ELEM( montageSeq->elem_size, MontageReader );
}
cvClearSeq(montageSeq);
numPolygonsInMontage=0;
//printf("Loading a montage with %d polygons on the protocol\n.",montage->total);
/** Load the montage onto the step object**/
cvSeqPush(myP->Steps,&montage);
/** Progress to the next step **/
CV_NEXT_SEQ_ELEM( stepSeq->elem_size, StepReader );
}
return myP;
}
// Main function, defines the entry point for the program.
int main( int argc, char** argv )
{
// Structure for getting video from camera or avi
CvCapture* capture = 0;
// Images to capture the frame from video or camera or from file
IplImage *frame, *frame_copy = 0;
// Input file name for avi or image file.
const char* vectorList;
const char* testImage;
// Check for the correct usage of the command line
if( argc == 2 )
{
//vectorList = argv[1];
testImage = argv[1];
}
else
{
fprintf( stderr, "Usage: eigenDecomp testImage\n" );
return -1;
}
// Allocate the memory storage
storage = cvCreateMemStorage(0);
// Create a new named window with title: result
cvNamedWindow( "result", 1 );
std::vector<IplImage*> images;
std::vector<char*> labels;
//Load Labels
printf("Loading Labels... ");
FILE* f = fopen( "labels.txt", "rt" );
if( f )
{
char buf[100+1];
// Get the line from the file
while( fgets( buf, 100, f ) )
{
// Remove the spaces if any, and clean up the name
int len = (int)strlen(buf);
while( len > 0 && isspace(buf[len-1]) )
len--;
buf[len] = '\0';
char* str = (char*)malloc(sizeof(char)*100);
memcpy(str, buf, 100);
labels.push_back(str);
}
// Close the file
fclose(f);
printf("%d Labels loaded.\n", labels.size());
} else
printf("Failed.\n");
//Load Eigenvectors:
printf("Loading Eigenvectors... ");
CvFileStorage* fs2 = cvOpenFileStorage("eigenvectors.yml", NULL, CV_STORAGE_READ);
char vectorname[50];
CvFileNode* vectorloc = cvGetFileNodeByName(fs2, NULL, "vector0");
for(int i = 1; vectorloc != NULL; i++) {
images.push_back((IplImage*)cvRead(fs2, vectorloc, &cvAttrList(0,0)));
//printf("pushed %s\n", vectorname);
sprintf(vectorname, "vector%d", i);
vectorloc = cvGetFileNodeByName(fs2, NULL, vectorname);
}
//cvReleaseFileStorage(&fs2); This may delete the images
printf("%d Eigenvectors (and 1 average) loaded.\n", images.size()-1);
//TODO: unfix this number! - Done!
//printf("FLANN DIMS: %d, %d\n", 165, images.size());
//cv::Mat mat( 165, images.size(), CV_32F );
//flann::Matrix<float> flannmat;
//flann::load_from_file(flannmat, "flann.dat", "flann.dat");
//flann::Index::Index flannIndex(flannmat, flann::LinearIndexParams());
printf("Loading Nearest Neighbor Matrix... ");
CvMat* flannmat;
CvFileStorage* fs = cvOpenFileStorage("nn.yml", NULL, CV_STORAGE_READ);
flannmat = (CvMat*)cvRead(fs, cvGetFileNodeByName(fs, NULL, "data"), &cvAttrList(0,0));
//cvReleaseFileStorage(&fs); This will delete the matrix
printf("Done. DIMS: %d, %d\n", flannmat->rows, flannmat->cols);
cv::flann::Index::Index flannIndex(flannmat, cv::flann::LinearIndexParams());
int nn_dims = flannmat->cols; //number of nearest neighbor dimensions available
int projection_dims = images.size()-1; //number of dimensions to project into eigenspace.
IplImage* eigenArray[projection_dims];
IplImage* avgImage = images[0];
for(int i = 0; i < projection_dims; i++) {
eigenArray[i] = images[i+1];
}
//load test image
printf("Loading Test Image...\n");
IplImage* testImg = cvLoadImage(testImage, CV_LOAD_IMAGE_GRAYSCALE);
float projection[projection_dims];
// Project the test image onto the PCA subspace
printf("Conducting Eigen Decomposite...\n");
cvEigenDecomposite(
testImg, //test object
projection_dims, //number of eigen vectors
(void*)eigenArray, //eigenVectors
0, 0, //ioflags, user callback data
avgImage, //root eigen vector
projection);
//print projection
//printf("Eigenvector Coefficents:\n");
//for(int i = 0; i < projection_dims; i++)
//printf("%5f ", projection[i]);
//printf("\n");
int neighbors = 10; //to test against
std::vector<float> proj(nn_dims);
std::vector<float> dists(neighbors);
std::vector<int> indicies(neighbors);
for(int i = 0; i < nn_dims; i++)
proj[i] = projection[i];
flannIndex.knnSearch(proj, indicies, dists, neighbors, NULL);
for(int i = 0; i < neighbors; i++)
printf("Index Match0: %4d, dist: %13.3f, Label: %s\n",
indicies[i], dists[i], labels[indicies[i]]);
// Destroy the window previously created with filename: "result"
cvDestroyWindow("result");
printf("Done.\n");
// return 0 to indicate successfull execution of the program
return 0;
}
CV_IMPL void*
cvLoad( const char* filename, CvMemStorage* memstorage,
const char* name, const char** _real_name )
{
void* ptr = 0;
const char* real_name = 0;
cv::FileStorage fs(cvOpenFileStorage(filename, memstorage, CV_STORAGE_READ));
CvFileNode* node = 0;
if( !fs.isOpened() )
return 0;
if( name )
{
node = cvGetFileNodeByName( *fs, 0, name );
}
else
{
int i, k;
for( k = 0; k < (*fs)->roots->total; k++ )
{
CvSeq* seq;
CvSeqReader reader;
node = (CvFileNode*)cvGetSeqElem( (*fs)->roots, k );
CV_Assert(node != NULL);
if( !CV_NODE_IS_MAP( node->tag ))
return 0;
seq = node->data.seq;
node = 0;
cvStartReadSeq( seq, &reader, 0 );
// find the first element in the map
for( i = 0; i < seq->total; i++ )
{
if( CV_IS_SET_ELEM( reader.ptr ))
{
node = (CvFileNode*)reader.ptr;
goto stop_search;
}
CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
}
}
stop_search:
;
}
if( !node )
CV_Error( CV_StsObjectNotFound, "Could not find the/an object in file storage" );
real_name = cvGetFileNodeName( node );
ptr = cvRead( *fs, node, 0 );
// sanity check
if( !memstorage && (CV_IS_SEQ( ptr ) || CV_IS_SET( ptr )) )
CV_Error( CV_StsNullPtr,
"NULL memory storage is passed - the loaded dynamic structure can not be stored" );
if( cvGetErrStatus() < 0 )
{
cvRelease( (void**)&ptr );
real_name = 0;
}
if( _real_name)
{
if (real_name)
{
*_real_name = (const char*)cvAlloc(strlen(real_name));
memcpy((void*)*_real_name, real_name, strlen(real_name));
} else {
*_real_name = 0;
}
}
return ptr;
}
