AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
conversions.cpp
1 #include "header/conversions.h"
2 
3 std::vector<double> CONV::tfMat3x3_to_vector(tf::Matrix3x3 matrix3x3){
4 
5  std::vector<double> vector(9);
6  vector[0] = matrix3x3.getColumn(0).getX();
7  vector[1] = matrix3x3.getColumn(0).getY();
8  vector[2] = matrix3x3.getColumn(0).getZ();
9  vector[3] = matrix3x3.getColumn(1).getX();
10  vector[4] = matrix3x3.getColumn(1).getY();
11  vector[5] = matrix3x3.getColumn(1).getZ();
12  vector[6] = matrix3x3.getColumn(2).getX();
13  vector[7] = matrix3x3.getColumn(2).getY();
14  vector[8] = matrix3x3.getColumn(2).getZ();
15 
16  return vector;
17 
18 }
19 
20 Eigen::VectorXd CONV::vector_std2Eigen(std::vector<double> vect){
21 
22  Eigen::VectorXd vec_eig(vect.size());
23 
24  for (int i=0; i<vect.size(); i++){
25  vec_eig(i) = vect.at(i);
26  }
27 
28  return vec_eig;
29 }
30 
31 std::vector<double> CONV::vector_cmat2std(CMAT::Matrix mat){
32 
33  std::vector<double> vect;
34  if (mat.GetNumColumns() != 1){
35  std::cout << "[CONV] vector_cmat2std ERROR: argument must be a nx1 matrix (ie, a vector)";
36  return vect;
37  }
38  vect.resize(mat.GetNumRows());
39 
40  int i = 1;
41  for(std::vector<double>::iterator it = vect.begin(); it != vect.end(); ++it) {
42  *it = mat(i);
43  i++;
44  }
45 
46  return vect;
47 }
48 
49 std::vector<double> CONV::vector_eigen2std(Eigen::VectorXd vect){
50 
51  std::vector<double> vect_std(vect.size());
52  for (int i=0; i< vect.size(); i++){
53  vect_std.at(i) = vect(i);
54  }
55 
56  return vect_std;
57 }
58 
59 Eigen::Matrix3d CONV::rotMatrix_cmat2eigen(CMAT::RotMatrix mat_cmat){
60 
61  Eigen::Matrix3d mat_eigen;
62  mat_eigen << mat_cmat(1,1), mat_cmat(1,2), mat_cmat(1,3),
63  mat_cmat(2,1), mat_cmat(2,2), mat_cmat(2,3),
64  mat_cmat(3,1), mat_cmat(3,2), mat_cmat(3,3);
65 
66  return mat_eigen;
67 }
68 
69 
70 Eigen::Matrix4d CONV::transfMatrix_cmat2eigen(CMAT::TransfMatrix mat_cmat){
71 
72  Eigen::Matrix4d mat_eigen = Eigen::Matrix4d::Identity();
73  mat_eigen.topLeftCorner<3,3>() = CONV::rotMatrix_cmat2eigen(mat_cmat.GetRotMatrix());
74  CMAT::Vect3 trasl = mat_cmat.GetTrasl();
75  mat_eigen(0,3) = trasl(1);
76  mat_eigen(1,3) = trasl(2);
77  mat_eigen(2,3) = trasl(3);
78 
79 
80  return mat_eigen;
81 }
82 
83 Eigen::MatrixXd CONV::matrix_cmat2eigen(CMAT::Matrix mat_cmat){
84 
85  Eigen::MatrixXd mat_eigen(mat_cmat.GetNumRows(), mat_cmat.GetNumColumns());
86 
87  for (int i = 0; i<mat_cmat.GetNumRows(); i++){
88  for (int j = 0; j<mat_cmat.GetNumColumns(); j++){
89  mat_eigen(i,j) = mat_cmat(i+1, j+1);
90  }
91  }
92 
93  return mat_eigen;
94 
95 }
96 
97 //CMAT::TransfMatrix CONV::transfMatrix_tf2cmat(tf::Transform mat_tf){
98 
99 // std::vector<double> vector = CONV::tfMat3x3_to_vector(mat_tf.getBasis());
100 // CMAT::RotMatrix rot_cmat(&vector[0]);
101 // double* lin = mat_tf.getOrigin();
102 // CMAT::Vect3 orient_cmat(lin);
103 
104 // CMAT::TransfMatrix mat_cmat(rot_cmat, orient_cmat);
105 
106 // return mat_cmat;
107 //}
108 
109 CMAT::Matrix CONV::matrix_eigen2cmat(Eigen::MatrixXd mat_eigen){
110  CMAT::Matrix mat_cmat(mat_eigen.rows(), mat_eigen.cols(), mat_eigen.data());
111  return mat_cmat;
112 }
113 
114 
115 Eigen::Matrix4d CONV::transfMatrix_tf2eigen(tf::Transform mat_tf){
116  Eigen::Matrix4d mat_eigen = Eigen::Matrix4d::Identity();
117 
118  //rotational part
119  Eigen::Matrix3d rot_eigen;
120  tf::matrixTFToEigen(mat_tf.getBasis(), rot_eigen);
121  mat_eigen.topLeftCorner<3,3>() = rot_eigen;
122 
123  //trans part
124  mat_eigen(0,3) = mat_tf.getOrigin().getX();
125  mat_eigen(1,3) = mat_tf.getOrigin().getY();
126  mat_eigen(2,3) = mat_tf.getOrigin().getZ();
127 
128 
129  return mat_eigen;
130 }
131 
132 //tf::Transform CONV::transfMatrix_eigen2tf(Eigen::Matrix4d mat_eigen){
133 // tf::Transform mat_tf;
134 // tf::transformEigenToTF(mat_eigen, mat_tf);
135 // return mat_tf;
136 //}
137 
144 Eigen::Matrix4d CONV::transfMatrix_kdl2eigen(KDL::Frame mat_kdl){
145 
146  Eigen::Matrix4d mat_eigen = Eigen::Matrix4d::Identity();
147 
148  // with << operand we have to indicate elements row by row. kdl store matrix rowMajor so
149  // we can do this
150  mat_eigen.topLeftCorner<3,3>() << mat_kdl.M.data[0], mat_kdl.M.data[1], mat_kdl.M.data[2],
151  mat_kdl.M.data[3], mat_kdl.M.data[4], mat_kdl.M.data[5],
152  mat_kdl.M.data[6], mat_kdl.M.data[7], mat_kdl.M.data[8];
153 
154  mat_eigen(0,3) = mat_kdl.p.x();
155  mat_eigen(1,3) = mat_kdl.p.y();
156  mat_eigen(2,3) = mat_kdl.p.z();
157 
158  return mat_eigen;
159 }
160 
169 Eigen::MatrixXd CONV::jacobian_kdl2eigen(KDL::Jacobian mat_kdl){
170  Eigen::MatrixXd mat_eigen = mat_kdl.data;
171  return mat_eigen;
172 }
173 
180 KDL::Frame CONV::transfMatrix_eigen2kdl(Eigen::Matrix4d mat_eigen){
181 
182  KDL::Frame mat_kdl; //default construct create identity
183 
184 
185  //REMEMBER: kdl is ROW MAJOR, EIGEN is COLUMN MAJOR
186  Eigen::Matrix3d rot_eigen = mat_eigen.topLeftCorner<3,3>();
187 
188  mat_kdl.M.data[0] = rot_eigen(0,0);
189  mat_kdl.M.data[1] = rot_eigen(0,1);
190  mat_kdl.M.data[2] = rot_eigen(0,2);
191  //kdl second row
192  mat_kdl.M.data[3] = rot_eigen(1,0);
193  mat_kdl.M.data[4] = rot_eigen(1,1);
194  mat_kdl.M.data[5] = rot_eigen(1,2);
195  //kdl third row
196  mat_kdl.M.data[6] = rot_eigen(2,0);
197  mat_kdl.M.data[7] = rot_eigen(2,1);
198  mat_kdl.M.data[8] = rot_eigen(2,2);
199 
200  //trasl part
201  mat_kdl.p.x(mat_eigen(0,3));
202  mat_kdl.p.y(mat_eigen(1,3));
203  mat_kdl.p.z(mat_eigen(2,3));
204 
205 
206 
207 
208  return mat_kdl;
209 
210 }
211 
212 CMAT::Matrix CONV::matrix_visp2cmat(vpMatrix mat_visp){
213  CMAT::Matrix mat_cmat(mat_visp.getRows(), mat_visp.getCols());
214  for (int i=0; i<mat_visp.getRows(); i++){
215  for (int j=0; j<mat_visp.getCols(); j++){
216  mat_cmat(i+1, j+1) = mat_visp[i][j];
217  }
218  }
219 
220  return mat_cmat;
221 
222 
223 }
224 
225 Eigen::MatrixXd CONV::matrix_visp2eigen(vpMatrix mat_visp){
226 
227  Eigen::MatrixXd mat_eigen(mat_visp.getRows(), mat_visp.getCols());
228  for (int i=0; i<mat_visp.getRows(); i++){
229  for (int j=0; j<mat_visp.getCols(); j++){
230  mat_eigen(i,j) = mat_visp[i][j];
231  }
232  }
233 
234  return mat_eigen;
235 }
236 
237 vpHomogeneousMatrix CONV::transfMatrix_eigen2visp(Eigen::MatrixXd mat_eigen){
238  vpHomogeneousMatrix mat_visp;
239  for (int i=0; i<mat_eigen.rows(); i++){
240  for (int j=0; j<mat_eigen.cols(); j++){
241  mat_visp[i][j] = mat_eigen(i,j);
242  }
243  }
244  return mat_visp;
245 }
246 
248 //CMAT::Matrix CONV::matrix_opencv2cmat(cv::Mat1d mat_opencv){
249 
250 // CMAT::Matrix mat_cmat(mat_opencv.rows, mat_opencv.cols);
251 // for (int i=0; i<mat_opencv.rows; i++){
252 // for (int j=0; j<mat_opencv.cols; j++){
253 // mat_cmat(i+1, j+1) = mat_opencv.at<double>(i,j);
254 // }
255 // }
256 
257 // return mat_cmat;
258 
259 //}
260 
261 
262 geometry_msgs::Transform CONV::transfMatrix_eigen2geomMsgs(Eigen::Matrix4d mat_eigen){
263 
264  geometry_msgs::Transform transf_ros;
265 
266 
267  transf_ros.translation.x = mat_eigen(0,3);
268  transf_ros.translation.y = mat_eigen(1,3);
269  transf_ros.translation.z = mat_eigen(2,3);
270 
271  Eigen::Quaterniond quat_eigen(mat_eigen.topLeftCorner<3,3>());
272  transf_ros.rotation.x = quat_eigen.x();
273  transf_ros.rotation.y = quat_eigen.y();
274  transf_ros.rotation.z = quat_eigen.z();
275  transf_ros.rotation.w = quat_eigen.w();
276 
277  return transf_ros;
278 
279 }
280 
281 Eigen::Matrix4d CONV::transfMatrix_geomMsgs2Eigen(geometry_msgs::Transform mat_msgs){
282 
283  Eigen::Matrix4d mat_eigen = Eigen::Matrix4d::Identity();
284 
285  mat_eigen(0,3) = mat_msgs.translation.x;
286  mat_eigen(1,3) = mat_msgs.translation.y;
287  mat_eigen(2,3) = mat_msgs.translation.z;
288 
289  Eigen::Quaterniond quat_eigen;
290  quat_eigen.x() = mat_msgs.rotation.x;
291  quat_eigen.y() = mat_msgs.rotation.y;
292  quat_eigen.z() = mat_msgs.rotation.z;
293  quat_eigen.w() = mat_msgs.rotation.w;
294 
295  //The quaternion is required to be normalized, otherwise the result is undefined.
296  mat_eigen.topLeftCorner<3,3>() = quat_eigen.normalized().toRotationMatrix();
297 
298  return mat_eigen;
299 
300 }
301 
302 
Generated by   doxygen 1.8.11