utils.cpp
2.66 KB
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
#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 \n";
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 << ";\n";
else myfile << " ];\n";
}
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();
}