tristan cardot / Pollen3D

Blame view

src/utils.cpp 2.66 KB
edit raw normal view history
b0bb08d1c   tristan   init 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
 
  #include "utils.h"
  
  void util::saveFileToMatlab(std::string fileName, cv::Mat a, std::string varName){
      int rows = (int) a.rows;
      int cols = (int) a.cols;
  
      std::ofstream myfile;
      myfile.open (fileName);
      myfile << "%File generated with Pollen3D 
  ";
  
      myfile << varName << " = [";
  
      for (int row = 0; row < rows; row++){
          for (int col = 0; col < cols; col++){
              myfile << a.at<double>(row,col) << " ";
          }
          if ( row != rows - 1) myfile << ";
  ";
          else                  myfile << " ];
  ";
      }
  
      myfile.close();
  }
  
  
  double util::rad2deg(double angRad){
      return angRad / PI * 180.0;
  }
  
  double util::deg2rad(double angDeg){
      return angDeg / 180.0 * PI;
  }
  
  /*! Make matrix non-homogenious. It is mainly used for measurement matrix \f$W\f$.
   * If ((W mod 3) == 0) it will supress every third row
   */
  void util::makeNonHomogenious(cv::Mat &m)
  {
      // Make W non-homogenious
      bool isNonHomogenious = !( (m.rows % 3) == 0 && (m.at<double>(2,0) == 1));
      if (isNonHomogenious)
          return;
  
      cv::Mat newM;
  
      for (int i = 0 ; i < m.rows / 3 ; i++){
          newM.push_back(m.row(3*i));
          newM.push_back(m.row(3*i + 1));
      }
  
      m.release();
      m = newM.clone();
  }
  
  /*! Copy elements of matrix with indices idx to vector
   * Equivalent to MATLAB: v = M(idx);
   */
  void util::copyMatElementsToVector(const cv::Mat &mat, const cv::Mat &idx, std::vector<double> &vec)
  {
      int nbIdx = (int) idx.total();
  
      for (int i = 0; i < nbIdx; i++){
          int col = (int) idx.at<cv::Point>(i).x;
          int row = (int) idx.at<cv::Point>(i).y;
  
          vec[i] = mat.at<double>(row,col);
      }
  }
  
  /*! Copy vector to matrix elements with indices idx
   * Equivalent to MATLAB: M(idx) = v;
   */
  void util::copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat)
  {
      int nbIdx = (int) idx.total();
  
      for (int i = 0; i < nbIdx; i++){
          int col = (int) idx.at<cv::Point>(i).x;
          int row = (int) idx.at<cv::Point>(i).y;
  
          mat.row(row).col(col) = vec[i];
      }
  }
  
  /*! Concatenate vector of matrices
   * \param matArray vector of matrices
   * \param method concatenation method
   * - CONCAT_HORIZONTAL (Horizontally) : M = [M1 M2 M3 M4]
   * - CONCAT_VERTICAL (Vertically)  : M = [M1;M2;M3;M4]
   */
  cv::Mat util::concatenateMat(const std::vector<cv::Mat> &matArray, int method)
  {
      cv::Mat outMat;
      if (method == CONCAT_VERTICAL){
          for(auto& m:matArray){
              outMat.push_back(m.clone());
          }
      }else{
          for(auto& m:matArray){
              cv::Mat mt = m.t();
              outMat.push_back(mt.clone());
          }
          outMat = outMat.t();
      }
      return outMat.clone();
  }