ulvis.paste.net

Paste Search Dynamic
Recent pastes
RIM_first attempt
  1. #include <QCoreApplication>
  2.  
  3. #include <iostream>
  4. #include <cmath>
  5.  
  6. #include <opencv2/opencv.hpp>
  7. #include<opencv2/core/core.hpp>
  8. #include<opencv2/highgui/highgui.hpp>
  9. #include<opencv2/imgproc/imgproc.hpp>
  10.  
  11. #define PI 3.14159265
  12. #define rad PI / 180.0
  13.  
  14. float planes[5][4];                     //5 planes, A C B D - parametrs
  15.  
  16. int     MatX, MatZ;
  17. float   x_w, y_w, z_w, x_0, y_0, z_0;
  18. float   x_i[206], z_i[156];
  19. float   Alpha_H, Betta_V;
  20. float   a_d,  a_i[206], b_i[156];       //a_d minimal angle
  21.  
  22. float   l00;                            //middle distance
  23. //float x_ort[206], z_ort[156];
  24. float   t[5][206][156];
  25. float   intersection[5][206][156][3];       //[plane][x][y][xyz]cross_dot
  26.  
  27. float   dist_mat[206][156];
  28. cv::Mat frame, grey_frame;
  29.  
  30. using namespace cv; // !!!!!!!!!!
  31. using namespace std;
  32.  
  33. int main(int argc, char *argv[])
  34. {
  35.     QCoreApplication a(argc, argv);
  36.  
  37.     int i, j, k;
  38.  
  39.     // enter initial data
  40. /*  for (i = 0; i < 5; i++)
  41.     {
  42.         std::cout << "Enter " << i << " wall (plane) equation Ax+By+Cz+D = 0;\n";
  43.         std::cout   << "A = ";
  44.         std::cin    >> planes[i][0];
  45.         std::cout   << "B = ";
  46.         std::cin    >> planes[i][1];
  47.         std::cout   << "C = ";
  48.         std::cin    >> planes[i][2];
  49.         std::cout   << "D = ";
  50.         std::cin    >> planes[i][3];
  51.     }*/
  52.     planes[2][0] = 0;
  53.     planes[2][1] = 1;
  54.     planes[2][2] = 0;
  55.     planes[2][3] = -10000;  //10m in mm
  56.  
  57.     // Mattrix size: MatX*MatZ
  58.     std::cout   << "Enter Matrix size L*H;\n";
  59.     std::cout   << "L = ";
  60. //  std::cin    >> MatX;
  61.     std::cout   << "H = ";
  62. //  std::cin    >> MatZ;
  63.     MatX = 206; MatZ = 156;
  64.  
  65.     // Angle: Alpha_H Betta_V
  66.     std::cout   << "Enter 1-pix angle*angle;\n";
  67.     std::cout   << "angle = (0.141)";
  68. //  std::cin    >> a_d;
  69.     a_d = 0.141;
  70.     std::cout << "Matrix angle*angle = " << a_d*MatX << "x" << a_d*MatZ;
  71.  
  72.     // View dot (x0, y0, z0)
  73. /*  std::cout   << "Enter wiev dot coordinates (x, y, z);\n";
  74.     std::cout   << "x = ";
  75.     std::cin    >> x_w;
  76.     std::cout   << "y = ";
  77.     std::cin    >> y_w;
  78.     std::cout   << "z = ";
  79.     std::cin    >> z_w;*/
  80.     x_w = 0000;     //2m - middle in 4m width //x_0 before
  81.     y_w = 0;
  82.     z_w = 0000;     //1.5m - middle in 3m height
  83.  
  84.     std::cout << "\n";
  85.     //middle (0;0) distance
  86.     l00 = -(planes[2][0] * x_w + planes[2][2] * z_w + planes[2][3]) / planes[2][1];
  87.     std::cout << "l00 = "<< l00;
  88.     x_0 = x_w; y_0 = l00; z_0 = z_w;    //on ortoogonal plane
  89.  
  90.     std::cout << "\n";
  91. /*  //Angle table counting
  92.     for (i = 0; i < 103; i++) {
  93.         a_i[i] = -(0.5 + (102 - i))*a_d;
  94.     }
  95.     for (i = 103; i < 206; i++) {
  96.         a_i[i] = (0.5 + (i - 103))*a_d;
  97.     }
  98.     for (i = 0; i < 78; i++) {
  99.         b_i[i] = -(0.5 + (77 - i))*a_d;
  100.     }
  101.     for (i = 78; i < 156; i++) {
  102.         b_i[i] = (0.5 + (i - 78))*a_d;
  103.     }
  104. */
  105.     //Tantent table ortogonal table
  106.     for (i = 0; i < 103; i++) {
  107.         a_i[i] = tan(-(0.5 + (102 - i))*a_d*rad);
  108. //      std::cout << " " << a_i[i];
  109.     }
  110.     std::cout << "\nmiddle\n";
  111.     for (i = 103; i < 206; i++) {
  112.         a_i[i] = tan((0.5 + (i - 103))*a_d*rad);
  113. //      std::cout << " " << a_i[i];
  114.     }
  115.     for (i = 0; i < 78; i++) {
  116.         b_i[i] = tan(-(0.5 + (77 - i))*a_d*rad);
  117.     }
  118.     for (i = 78; i < 156; i++) {
  119.         b_i[i] = tan((0.5 + (i - 78))*a_d*rad);
  120.     }
  121.     //tan b test
  122.     std::cout << "\nTan B test\n";
  123.     for (i = 0; i < 156; i=i+10) {
  124.         std::cout << "  "<< b_i[i];
  125.     }
  126.  
  127.     std::cout << "\n ORTOGONAL\n";
  128.     //Xi Zi ortogonal table
  129.     for (i = 0; i < 206; i++) {
  130.         x_i[i] = x_0 + l00*a_i[i];
  131.     }
  132.     for (i = 0; i < 156; i++) {
  133.         z_i[i] = z_0 + l00*b_i[i];
  134.     }
  135.     //Ortan table test
  136.     std::cout << "\n Ortan Z-table test\n";
  137.     for (i = 0; i < 156; i = i + 10) {
  138.         std::cout << "  " << z_i[i];
  139.     }
  140.  
  141.     //Line-equation coefficient
  142.     for (i = 2; i < 3; i++)     //i=0; i<5
  143.     {
  144.         for (j = 0; j < 206; j++)
  145.         {
  146.             for (k = 0; k < 156; k++)
  147.             {
  148.                 t[i][j][k] = (planes[i][0] * x_w + planes[i][1] * y_w + planes[i][2] * z_w + planes[i][3]) /
  149.                     ((planes[i][0] * (x_w - x_i[j])) + (planes[i][1] * (y_w - l00)) + (planes[i][2] * (z_w - z_i[k])));
  150.             }
  151.         }
  152.     }
  153.     //Line-equation coefficient test
  154.     std::cout << "\n\n Line-equation Z coefficient test\n";
  155.     for (i = 0; i < 156; i = i + 10) {
  156.         std::cout << "  " << t[2][100][i];
  157.     }
  158.     std::cout << "\n Line-equation X coefficient test\n";
  159.     for (i = 0; i < 206; i = i + 10) {
  160.         std::cout << "  " << t[2][i][70];
  161.     }
  162.  
  163.     std::cout << "\nINTERSECTION TEST Z\n";
  164.     //intersection table count [205][155][3]
  165.     for (i = 2; i < 3; i++)
  166.     {
  167.         for (j = 0; j < 206; j++)
  168.         {
  169.             for (k = 0; k < 156; k++)
  170.             {
  171.                 intersection[i][j][k][0] = x_w + (x_i[j] - x_w)*t[i][j][k]; // x
  172.                 intersection[i][j][k][1] = y_w + (l00    - y_w)*t[i][j][k]; // y
  173.                 intersection[i][j][k][2] = z_w + (z_i[k] - z_w)*t[i][j][k]; // z
  174.                 if (j==100) std::cout << "  " << intersection[i][j][k][2];
  175.             }
  176. //          std::cout << "\n";
  177.         }
  178.     }
  179.     //intersection table test
  180.     std::cout << "\n\n intersection Z table test\n";
  181.     for (i = 0; i < 156; i = i + 10) {
  182.         std::cout << "  " << intersection[2][100][i][2];
  183.     }
  184.     std::cout << "\n\n intersection X table test\n";
  185.     for (i = 0; i < 206; i = i + 10) {
  186.         std::cout << "  " << intersection[2][i][70][0];
  187.     }
  188.  
  189.     for (i = 2; i < 3; i++)
  190.     {
  191.         for (j = 0; j < 206; j++)
  192.         {
  193.             for (k = 0; k < 156; k++)
  194.             {
  195.                 dist_mat[j][k] = sqrt(pow(intersection[i][j][k][0] - x_w, 2) + pow(intersection[i][j][k][1] - y_w, 2) + pow(intersection[i][j][k][2] - z_w, 2));
  196.  
  197.             }
  198.         }
  199.     }
  200.  
  201.     std::cout << "\n\nFINAL\n";
  202.     for (i = 0; i < 206; i = i + 10)
  203.     {
  204.         std::cout << "        " << i;
  205.     }
  206.     std::cout << "\n";
  207.     for (k = 0; k < 156; k = k + 10)
  208.     {
  209.         std::cout << k;
  210.         for (j = 0; j < 206; j = j + 10)
  211.         {
  212.             std::cout << "   " << dist_mat[j][k];//\t
  213.         }
  214.         std::cout << "\n";
  215.     }
  216. // /////////////////////////////////////
  217.     float min=20000, max=0;
  218.  
  219.     for (j = 0; j < 206; j++)
  220.     {
  221.         for (k = 0; k < 156; k++)
  222.         {
  223.             if (dist_mat[j][k] > max) max = dist_mat[j][k];
  224.             if (dist_mat[j][k] < min) min = dist_mat[j][k];
  225.         }
  226.     }
  227.     std::cout << "\nmin = " << min;
  228.     std::cout << "\nmax = " << max;
  229.     std::cout << "\n !!!";
  230.  
  231.     unsigned int dist_mat2[256][156];
  232.     for (j = 0; j < 206; j++)
  233.     {
  234.         for (k = 0; k < 156; k++)
  235.         {
  236.             dist_mat2[j][k] = (static_cast<unsigned int>(dist_mat[j][k]-10000)/3);
  237.  
  238.         }
  239.     }
  240.  
  241.     cout << "\n convert result= " << dist_mat2[100][40];
  242.  
  243.     // CvMat
  244.     frame = Mat(MatZ, MatX, CV_8UC1, Scalar(0)); //,0,0)
  245.     for (j = 0; j < 206; j++)        //frame.rows
  246.     {
  247.         for (k = 0; k < 156; k++)
  248.         {
  249.             frame.at<uchar>(k,j) = dist_mat2[k][j];
  250.         }
  251.     }
  252.  
  253.  
  254.     imshow("test",frame);
  255.     waitKey(0);
  256.     destroyWindow("test");
  257.  
  258.  
  259.     std::cout << "\n !!!"; //this wont be displayed cos idk why
  260.     return a.exec();
  261. }
Parsed in 0.060 seconds