SampleOptimization.cpp
Go to the documentation of this file.
00001 #include "Optimization.h"
00002 #include "cv.h"
00003 #include "highgui.h"
00004 #include <time.h>
00005 #include <vector>
00006 #include <iostream>
00007 #include <string>
00008 using namespace std;
00009 using namespace alvar;
00010 
00011 const int res=640;
00012 const double poly_res=8.0;
00013 
00014 double random(int dist_type, double param1, double param2) {
00015     static CvRNG rng=0;
00016     if (rng == 0) rng = cvRNG(time(0));
00017     double m_data;
00018     CvMat m = cvMat(1, 1, CV_64F, &m_data);
00019     cvRandArr(&rng, &m, dist_type, cvScalar(param1), cvScalar(param2));
00020     return m_data;
00021 }
00022 
00023 double get_y(double x, double a, double b, double c, double d, double e) {
00024     return (a*x*x*x*x + b*x*x*x + c*x*x + d*x + e);
00025 }
00026 
00027 // Just generate some random data that can be used as sensor input
00028 bool get_measurement(double *x, double *y, double a, double b, double c, double d, double e) {
00029     double xx = random(CV_RAND_UNI, -(poly_res/2), +(poly_res/2)); //(rand()*poly_res/RAND_MAX)-(poly_res/2);
00030     double yy = get_y(xx, a, b, c, d, e);
00031     double ry = random(CV_RAND_NORMAL, 0, poly_res/8); //(rand()*(poly_res/4)/RAND_MAX)-(poly_res/8);
00032     yy += ry;
00033     *x = xx;
00034     *y = yy;
00035     if (*y < -(poly_res/2)) return false;
00036     if (*y >= (poly_res/2)) return false;
00037     return true;
00038 }
00039 
00040 void Estimate(CvMat* state, CvMat *projection, void *param) {
00041     double *measx=(double *)param;
00042     int data_degree = state->rows-1;
00043     double a = (data_degree >= 4? cvmGet(state, 4, 0) : 0);
00044     double b = (data_degree >= 3? cvmGet(state, 3, 0) : 0);
00045     double c = (data_degree >= 2? cvmGet(state, 2, 0) : 0);
00046     double d = (data_degree >= 1? cvmGet(state, 1, 0) : 0);
00047     double e = (data_degree >= 0? cvmGet(state, 0, 0) : 0);
00048     for (int i=0; i<projection->rows; i++) {
00049         cvmSet(projection, i, 0, get_y(measx[i], a, b, c, d, e));
00050     }
00051 }
00052 
00053 int main(int argc, char *argv[])
00054 {
00055     try {
00056         // Output usage message
00057         std::string filename(argv[0]);
00058         filename = filename.substr(filename.find_last_of('\\') + 1);
00059         std::cout << "SampleOptimization" << std::endl;
00060         std::cout << "==================" << std::endl;
00061         std::cout << std::endl;
00062         std::cout << "Description:" << std::endl;
00063         std::cout << "  This is an example of how to use the 'Optimization' class. Random data" << std::endl;
00064         std::cout << "  is generated and approximated using curves of increasing degrees." << std::endl;
00065         std::cout << std::endl;
00066         std::cout << "Usage:" << std::endl;
00067         std::cout << "  " << filename << std::endl;
00068         std::cout << std::endl;
00069         std::cout << "Keyboard Shortcuts:" << std::endl;
00070         std::cout << "  any key: cycle through datasets" << std::endl;
00071         std::cout << "  q: quit" << std::endl;
00072         std::cout << std::endl;
00073 
00074         // Processing loop
00075         IplImage *img = cvCreateImage(cvSize(res,res), IPL_DEPTH_8U, 3);
00076         cvNamedWindow("SampleOptimization");
00077         for (int data_degree=0; data_degree<5; data_degree++) {
00078             double a = (data_degree >= 4? random(CV_RAND_UNI, -0.5, 0.5) : 0);
00079             double b = (data_degree >= 3? random(CV_RAND_UNI, -0.5, 0.5) : 0);
00080             double c = (data_degree >= 2? random(CV_RAND_UNI, -0.5, 0.5) : 0);
00081             double d = (data_degree >= 1? random(CV_RAND_UNI, -0.5, 0.5) : 0);
00082             double e = (data_degree >= 0? random(CV_RAND_UNI, -0.5, 0.5) : 0);
00083             cvZero(img);
00084             vector<CvPoint2D32f> measvec;
00085             for (int i=0; i<1000; i++) {
00086                 double x, y;
00087                 if (get_measurement(&x, &y, a, b, c, d, e)) {
00088                     measvec.push_back(cvPoint2D32f(x, y));
00089                     x = (x*res/poly_res)+(res/2);
00090                     y = (y*res/poly_res)+(res/2);
00091                     cvCircle(img, cvPoint(int(x), int(y)), 1, CV_RGB(0,255,0));
00092                 }
00093             }
00094             cvShowImage("SampleOptimization", img);
00095             cvWaitKey(10);
00096             double measx[1000];
00097             CvMat *meas = cvCreateMat(measvec.size(), 1, CV_64F);
00098             for (size_t i=0; i<measvec.size(); i++) {
00099                 measx[i] = measvec[i].x;
00100                 cvmSet(meas, i, 0, measvec[i].y);
00101             }
00102             for (int degree=0; degree<5; degree++) 
00103             {
00104                 double param_data[5]={0};
00105                 CvMat param = cvMat(degree+1, 1, CV_64F, param_data);
00106                 Optimization opt(param.rows, meas->rows);
00107                 opt.Optimize(&param, meas, 0.1, 100, Estimate, measx);
00108                 double a = (degree >= 4? cvmGet(&param, 4, 0) : 0);
00109                 double b = (degree >= 3? cvmGet(&param, 3, 0) : 0);
00110                 double c = (degree >= 2? cvmGet(&param, 2, 0) : 0);
00111                 double d = (degree >= 1? cvmGet(&param, 1, 0) : 0);
00112                 double e = (degree >= 0? cvmGet(&param, 0, 0) : 0);
00113                 const int step=5;
00114                 for (int x2=step; x2<res; x2+=step) {
00115                     int x1 = x2-step;
00116                     double xx1 = (x1*poly_res/res)-(poly_res/2);
00117                     double xx2 = (x2*poly_res/res)-(poly_res/2);
00118                     double yy1 = get_y(xx1, a, b, c, d, e);
00119                     double yy2 = get_y(xx2, a, b, c, d, e);
00120                     int y1 = int((yy1*res/poly_res)+(res/2));
00121                     int y2 = int((yy2*res/poly_res)+(res/2));
00122                     cvLine(img, cvPoint(x1,y1), cvPoint(x2,y2), CV_RGB(degree*50,255-(degree*50),255));
00123                 }
00124                 cvShowImage("SampleOptimization", img);
00125                 cvWaitKey(10);
00126             }
00127             cvReleaseMat(&meas);
00128             cvShowImage("SampleOptimization", img);
00129             int key = cvWaitKey(0);
00130             if (key == 'q') {
00131                 break;
00132             }
00133         }
00134         cvReleaseImage(&img);
00135         return 0;
00136     }
00137     catch (const std::exception &e) {
00138         std::cout << "Exception: " << e.what() << endl;
00139     }
00140     catch (...) {
00141         std::cout << "Exception: unknown" << std::endl;
00142     }
00143 }


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 21:12:54