Visual Servoing Platform  version 3.6.1 under development (2024-11-12)
vpHomogeneousMatrix.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Homogeneous matrix.
32  */
33 
40 #include <visp3/core/vpException.h>
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/core/vpMatrix.h>
43 #include <visp3/core/vpPoint.h>
44 #include <visp3/core/vpQuaternionVector.h>
45 
46 BEGIN_VISP_NAMESPACE
52  : vpArray2D<double>(4, 4)
53 {
54  buildFrom(t, q);
55  (*this)[3][3] = 1.;
56 }
57 
61 vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D<double>(4, 4), m_index(0) { eye(); }
62 
68 {
69  *this = M;
70 }
71 
77  : vpArray2D<double>(4, 4), m_index(0)
78 {
79  buildFrom(t, tu);
80  (*this)[3][3] = 1.;
81 }
82 
88  : vpArray2D<double>(4, 4), m_index(0)
89 {
90  const unsigned int index_3 = 3;
91  insert(R);
92  insert(t);
93  (*this)[index_3][index_3] = 1.;
94 }
95 
100 {
101  const unsigned int index_0 = 0;
102  const unsigned int index_1 = 1;
103  const unsigned int index_2 = 2;
104  const unsigned int index_3 = 3;
105  const unsigned int index_4 = 4;
106  const unsigned int index_5 = 5;
107  buildFrom(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], p[index_5]);
108  (*this)[index_3][index_3] = 1.;
109 }
110 
154 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v) : vpArray2D<double>(4, 4), m_index(0)
155 {
156  buildFrom(v);
157  (*this)[3][3] = 1.;
158 }
159 
160 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
201 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::initializer_list<double> &list)
202  : vpArray2D<double>(4, 4), m_index(0)
203 {
204  if (list.size() == 12) {
205  std::copy(list.begin(), list.end(), data);
206  data[12] = 0.;
207  data[13] = 0.;
208  data[14] = 0.;
209  data[15] = 1.;
210  }
211  else if (list.size() == 16) {
212  std::copy(list.begin(), list.end(), data);
213  for (size_t i = 12; i < 15; ++i) {
214  if (std::fabs(data[i]) > std::numeric_limits<double>::epsilon()) {
216  "Cannot initialize homogeneous matrix. "
217  "List element %d (%f) should be 0.",
218  i, data[i]));
219  }
220  }
221  if (std::fabs(data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
223  "Cannot initialize homogeneous matrix. "
224  "List element 15 (%f) should be 1.",
225  data[15]));
226  }
227  }
228  else {
230  "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
231  list.size()));
232  }
233 
234  if (!isAnHomogeneousMatrix()) {
235  if (isAnHomogeneousMatrix(1e-3)) {
236  const unsigned int index_0 = 0;
237  const unsigned int index_1 = 1;
238  const unsigned int index_2 = 2;
239  const unsigned int index_4 = 4;
240  const unsigned int index_5 = 5;
241  const unsigned int index_6 = 6;
242  const unsigned int index_8 = 8;
243  const unsigned int index_9 = 9;
244  const unsigned int index_10 = 10;
245  // re-orthogonalize rotation matrix since the input is close to a valid rotation matrix
246  vpRotationMatrix R(*this);
247  R.orthogonalize();
248 
249  data[index_0] = R[index_0][index_0];
250  data[index_1] = R[index_0][index_1];
251  data[index_2] = R[index_0][index_2];
252  data[index_4] = R[index_1][index_0];
253  data[index_5] = R[index_1][index_1];
254  data[index_6] = R[index_1][index_2];
255  data[index_8] = R[index_2][index_0];
256  data[index_9] = R[index_2][index_1];
257  data[index_10] = R[index_2][index_2];
258  }
259  else {
260  throw(vpException(
262  "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
263  }
264  }
265 }
266 #endif
267 
311 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
312 {
313  buildFrom(v);
314  (*this)[3][3] = 1.;
315 }
316 
322 vpHomogeneousMatrix::vpHomogeneousMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
323  : vpArray2D<double>(4, 4), m_index(0)
324 {
325  buildFrom(tx, ty, tz, tux, tuy, tuz);
326  (*this)[3][3] = 1.;
327 }
328 
334 {
335  insert(tu);
336  insert(t);
337  return *this;
338 }
339 
345 {
346  insert(R);
347  insert(t);
348  return *this;
349 }
350 
355 {
356  const unsigned int index_0 = 0;
357  const unsigned int index_1 = 1;
358  const unsigned int index_2 = 2;
359  const unsigned int index_3 = 3;
360  const unsigned int index_4 = 4;
361  const unsigned int index_5 = 5;
362  vpTranslationVector tv(p[index_0], p[index_1], p[index_2]);
363  vpThetaUVector tu(p[index_3], p[index_4], p[index_5]);
364 
365  insert(tu);
366  insert(tv);
367  return *this;
368 }
369 
375 {
376  insert(t);
377  insert(q);
378  return *this;
379 }
380 
386 vpHomogeneousMatrix &vpHomogeneousMatrix::buildFrom(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
387 {
388  vpRotationMatrix R(tux, tuy, tuz);
389  vpTranslationVector t(tx, ty, tz);
390 
391  insert(R);
392  insert(t);
393  return *this;
394 }
395 
441 {
442  if ((v.size() != 12) && (v.size() != 16)) {
443  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
444  }
445 
446  for (unsigned int i = 0; i < 12; ++i) {
447  this->data[i] = static_cast<double>(v[i]);
448  }
449  return *this;
450 }
451 
497 {
498  if ((v.size() != 12) && (v.size() != 16)) {
499  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
500  }
501 
502  for (unsigned int i = 0; i < 12; ++i) {
503  this->data[i] = v[i];
504  }
505  return *this;
506 }
507 
514 {
515  for (int i = 0; i < 4; ++i) {
516  for (int j = 0; j < 4; ++j) {
517  rowPtrs[i][j] = M.rowPtrs[i][j];
518  }
519  }
520  return *this;
521 }
522 
545 {
547 
548  vpRotationMatrix R1, R2, R;
549  vpTranslationVector T1, T2, T;
550 
551  extract(T1);
552  M.extract(T2);
553 
554  extract(R1);
555  M.extract(R2);
556 
557  R = R1 * R2;
558 
559  T = (R1 * T2) + T1;
560 
561  p.insert(T);
562  p.insert(R);
563 
564  return p;
565 }
566 
589 {
590  (*this) = (*this) * M;
591  return (*this);
592 }
593 
602 {
603  const unsigned int val_4 = 4;
604  if (v.getRows() != val_4) {
606  "Cannot multiply a (4x4) homogeneous matrix by a "
607  "(%dx1) column vector",
608  v.getRows()));
609  }
610  vpColVector p(rowNum);
611 
612  p = 0.0;
613 
614  for (unsigned int j = 0; j < val_4; ++j) {
615  for (unsigned int i = 0; i < val_4; ++i) {
616  p[i] += rowPtrs[i][j] * v[j];
617  }
618  }
619 
620  return p;
621 }
622 
635 {
636  vpPoint aP;
637 
638  vpColVector v(4), v1(4);
639 
640  const unsigned int index_0 = 0;
641  const unsigned int index_1 = 1;
642  const unsigned int index_2 = 2;
643  const unsigned int index_3 = 3;
644 
645  v[index_0] = bP.get_X();
646  v[index_1] = bP.get_Y();
647  v[index_2] = bP.get_Z();
648  v[index_3] = bP.get_W();
649 
650  v1[index_0] = ((*this)[index_0][0] * v[0]) + ((*this)[index_0][1] * v[1]) + ((*this)[index_0][index_2] * v[index_2]) + ((*this)[index_0][index_3] * v[index_3]);
651  v1[index_1] = ((*this)[index_1][0] * v[0]) + ((*this)[index_1][1] * v[1]) + ((*this)[index_1][index_2] * v[index_2]) + ((*this)[index_1][index_3] * v[index_3]);
652  v1[index_2] = ((*this)[index_2][0] * v[0]) + ((*this)[index_2][1] * v[1]) + ((*this)[index_2][index_2] * v[index_2]) + ((*this)[index_2][index_3] * v[index_3]);
653  v1[index_3] = ((*this)[index_3][0] * v[0]) + ((*this)[index_3][1] * v[1]) + ((*this)[index_3][index_2] * v[index_2]) + ((*this)[index_3][index_3] * v[index_3]);
654 
655  v1 /= v1[3];
656 
657  // --comment: v1 equals M multiplied by v
658  aP.set_X(v1[index_0]);
659  aP.set_Y(v1[index_1]);
660  aP.set_Z(v1[index_2]);
661  aP.set_W(v1[index_3]);
662 
663  aP.set_oX(v1[index_0]);
664  aP.set_oY(v1[index_1]);
665  aP.set_oZ(v1[index_2]);
666  aP.set_oW(v1[index_3]);
667 
668  return aP;
669 }
670 
682 {
683  vpTranslationVector t_out;
684  const unsigned int index_0 = 0;
685  const unsigned int index_1 = 1;
686  const unsigned int index_2 = 2;
687  const unsigned int index_3 = 3;
688  t_out[index_0] = (((*this)[index_0][0] * t[0]) + ((*this)[index_0][1] * t[1]) + ((*this)[index_0][index_2] * t[index_2])) + (*this)[index_0][index_3];
689  t_out[index_1] = (((*this)[index_1][0] * t[0]) + ((*this)[index_1][1] * t[1]) + ((*this)[index_1][index_2] * t[index_2])) + (*this)[index_1][index_3];
690  t_out[index_2] = (((*this)[index_2][0] * t[0]) + ((*this)[index_2][1] * t[1]) + ((*this)[index_2][index_2] * t[index_2])) + (*this)[index_2][index_3];
691 
692  return t_out;
693 }
694 
710 {
711  return (vpHomogeneousMatrix((*this).getTranslationVector(), (*this).getRotationMatrix() * R));
712 }
713 
761 {
762  m_index = 0;
763  data[m_index] = val;
764  return *this;
765 }
766 
814 {
815  ++m_index;
816  if (m_index >= size()) {
818  "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
819  "with %d elements",
820  size(), m_index + 1));
821  }
822  data[m_index] = val;
823  return *this;
824 }
825 
826 /*********************************************************************/
827 
834 bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const
835 {
837  extract(R);
838 
839  const unsigned int index_0 = 0;
840  const unsigned int index_1 = 1;
841  const unsigned int index_2 = 2;
842  const unsigned int index_3 = 3;
843  const double epsilon = std::numeric_limits<double>::epsilon();
844  bool isLastRowOK = vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) &&
845  vpMath::nul((*this)[index_3][index_2], epsilon);
846  return R.isARotationMatrix(threshold) && isLastRowOK && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon);
847 }
848 
854 {
855  unsigned int l_size = size();
856  for (unsigned int i = 0; i < l_size; ++i) {
857  if (vpMath::isNaN(data[i])) {
858  return false;
859  }
860  }
861  return true;
862 }
863 
869 {
870  const unsigned int val_3 = 3;
871  for (unsigned int i = 0; i < val_3; ++i) {
872  for (unsigned int j = 0; j < val_3; ++j) {
873  R[i][j] = (*this)[i][j];
874  }
875  }
876 }
877 
882 {
883  const unsigned int index_0 = 0;
884  const unsigned int index_1 = 1;
885  const unsigned int index_2 = 2;
886  const unsigned int index_3 = 3;
887  t[index_0] = (*this)[index_0][index_3];
888  t[index_1] = (*this)[index_1][index_3];
889  t[index_2] = (*this)[index_2][index_3];
890 }
895 {
897  (*this).extract(R);
898  tu.buildFrom(R);
899 }
900 
905 {
907  (*this).extract(R);
908  q.buildFrom(R);
909 }
910 
915 {
916  const unsigned int val_3 = 3;
917  for (unsigned int i = 0; i < val_3; ++i) {
918  for (unsigned int j = 0; j < val_3; ++j) {
919  (*this)[i][j] = R[i][j];
920  }
921  }
922 }
923 
931 {
932  vpRotationMatrix R(tu);
933  insert(R);
934 }
935 
940 {
941  const unsigned int index_0 = 0;
942  const unsigned int index_1 = 1;
943  const unsigned int index_2 = 2;
944  const unsigned int index_3 = 3;
945  (*this)[index_0][index_3] = t[index_0];
946  (*this)[index_1][index_3] = t[index_1];
947  (*this)[index_2][index_3] = t[index_2];
948 }
949 
957 
973 {
975 
977  extract(R);
979  extract(T);
980 
982  RtT = -(R.t() * T);
983 
984  Mi.insert(R.t());
985  Mi.insert(RtT);
986 
987  return Mi;
988 }
989 
994 {
995  const unsigned int index_0 = 0;
996  const unsigned int index_1 = 1;
997  const unsigned int index_2 = 2;
998  const unsigned int index_3 = 3;
999  (*this)[index_0][index_0] = 1;
1000  (*this)[index_1][index_1] = 1;
1001  (*this)[index_2][index_2] = 1;
1002  (*this)[index_3][index_3] = 1;
1003 
1004  (*this)[index_0][index_1] = 0;
1005  (*this)[index_0][index_2] = 0;
1006  (*this)[index_0][index_3] = 0;
1007  (*this)[index_1][index_0] = 0;
1008  (*this)[index_1][index_2] = 0;
1009  (*this)[index_1][index_3] = 0;
1010  (*this)[index_2][index_0] = 0;
1011  (*this)[index_2][index_1] = 0;
1012  (*this)[index_2][index_3] = 0;
1013  (*this)[index_3][index_0] = 0;
1014  (*this)[index_3][index_1] = 0;
1015  (*this)[index_3][index_2] = 0;
1016 }
1017 
1033 
1034 void vpHomogeneousMatrix::save(std::ofstream &f) const
1035 {
1036  if (!f.fail()) {
1037  f << *this;
1038  }
1039  else {
1040  throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
1041  }
1042 }
1043 
1044 void vpHomogeneousMatrix::save(const std::string &filename) const
1045 {
1046  std::ofstream f;
1047  f.open(filename.c_str());
1048  save(f);
1049  f.close();
1050 }
1051 
1052 void vpHomogeneousMatrix::load(std::ifstream &f)
1053 {
1054  if (!f.fail()) {
1055  const unsigned int val_4 = 4;
1056  for (unsigned int i = 0; i < val_4; ++i) {
1057  for (unsigned int j = 0; j < val_4; ++j) {
1058  f >> (*this)[i][j];
1059  }
1060  }
1061  }
1062  else {
1063  throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
1064  }
1065 }
1066 
1067 void vpHomogeneousMatrix::load(const std::string &filename)
1068 {
1069  std::ifstream f;
1070  f.open(filename.c_str());
1071  load(f);
1072  f.close();
1073 }
1074 
1079 {
1080  vpRotationMatrix R(*this);
1081  R.orthogonalize();
1082 
1083  const unsigned int index_0 = 0;
1084  const unsigned int index_1 = 1;
1085  const unsigned int index_2 = 2;
1086  const unsigned int index_4 = 4;
1087  const unsigned int index_5 = 5;
1088  const unsigned int index_6 = 6;
1089  const unsigned int index_8 = 8;
1090  const unsigned int index_9 = 9;
1091  const unsigned int index_10 = 10;
1092 
1093  data[index_0] = R[index_0][index_0];
1094  data[index_1] = R[index_0][index_1];
1095  data[index_2] = R[index_0][index_2];
1096  data[index_4] = R[index_1][index_0];
1097  data[index_5] = R[index_1][index_1];
1098  data[index_6] = R[index_1][index_2];
1099  data[index_8] = R[index_2][index_0];
1100  data[index_9] = R[index_2][index_1];
1101  data[index_10] = R[index_2][index_2];
1102 }
1103 
1106 {
1107  vpPoseVector r(*this);
1108  std::cout << r.t();
1109 }
1110 
1115 void vpHomogeneousMatrix::convert(std::vector<float> &M)
1116 {
1117  M.resize(12);
1118  for (unsigned int i = 0; i < 12; ++i) {
1119  M[i] = static_cast<float>(this->data[i]);
1120  }
1121 }
1122 
1127 void vpHomogeneousMatrix::convert(std::vector<double> &M)
1128 {
1129  M.resize(12);
1130  for (unsigned int i = 0; i < 12; ++i) {
1131  M[i] = this->data[i];
1132  }
1133 }
1134 
1139 {
1141  this->extract(tr);
1142  return tr;
1143 }
1144 
1149 {
1150  vpRotationMatrix R;
1151  this->extract(R);
1152  return R;
1153 }
1154 
1160 {
1161  vpThetaUVector tu;
1162  this->extract(tu);
1163  return tu;
1164 }
1165 
1199 {
1200  if (j >= getCols()) {
1201  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
1202  }
1203  unsigned int nb_rows = getRows();
1204  vpColVector c(nb_rows);
1205  for (unsigned int i = 0; i < nb_rows; ++i) {
1206  c[i] = (*this)[i][j];
1207  }
1208  return c;
1209 }
1210 
1217 vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::vector<vpPoint> &p, const std::vector<vpPoint> &q)
1218 {
1219  const double N = static_cast<double>(p.size());
1220 
1221  vpColVector p_bar(3, 0.0);
1222  vpColVector q_bar(3, 0.0);
1223  size_t p_size = p.size();
1224  const unsigned int val_3 = 3;
1225  for (size_t i = 0; i < p_size; ++i) {
1226  for (unsigned int j = 0; j < val_3; ++j) {
1227  p_bar[j] += p.at(i).oP[j];
1228  q_bar[j] += q.at(i).oP[j];
1229  }
1230  }
1231 
1232  for (unsigned int j = 0; j < val_3; ++j) {
1233  p_bar[j] /= N;
1234  q_bar[j] /= N;
1235  }
1236 
1237  vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
1238  vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
1239 
1240  for (unsigned int i = 0; i < static_cast<unsigned int>(p_size); ++i) {
1241  for (unsigned int j = 0; j < val_3; ++j) {
1242  pc[i][j] = p.at(i).oP[j] - p_bar[j];
1243  qc[i][j] = q.at(i).oP[j] - q_bar[j];
1244  }
1245  }
1246 
1247  const vpMatrix pct_qc = pc.t() * qc;
1248  vpMatrix U = pct_qc, V;
1249  vpColVector W;
1250  U.svd(W, V);
1251 
1252  vpMatrix Vt = V.t();
1253  vpMatrix R = U * Vt;
1254  if (R.det() < 0) {
1255  const unsigned int index_0 = 0;
1256  const unsigned int index_1 = 1;
1257  const unsigned int index_2 = 2;
1258  Vt[index_2][index_0] *= -1.;
1259  Vt[index_2][index_1] *= -1.;
1260  Vt[index_2][index_2] *= -1.;
1261 
1262  R = U * Vt;
1263  }
1264 
1265  const vpColVector t = p_bar - (R * q_bar);
1266 
1268 }
1269 
1279 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1280 {
1281  vpMatrix meanR(3, 3);
1282  vpColVector meanT(3);
1283  vpRotationMatrix R;
1284  const unsigned int index_0 = 0;
1285  const unsigned int index_1 = 1;
1286  const unsigned int index_2 = 2;
1287  size_t vec_m_size = vec_M.size();
1288  for (size_t i = 0; i < vec_m_size; ++i) {
1289  R = vec_M[i].getRotationMatrix();
1290  meanR += (vpMatrix)R;
1291  meanT += (vpColVector)vec_M[i].getTranslationVector();
1292  }
1293  meanR /= static_cast<double>(vec_M.size());
1294  meanT /= static_cast<double>(vec_M.size());
1295 
1296  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1297  vpMatrix M, U, V;
1298  vpColVector sv;
1299  meanR.pseudoInverse(M, sv, 1e-6, U, V);
1300  double det = sv[index_0] * sv[index_1] * sv[index_2];
1301  if (det > 0) {
1302  meanR = U * V.t();
1303  }
1304  else {
1305  vpMatrix D(3, 3);
1306  D = 0.0;
1307  D[index_0][index_0] = 1.0;
1308  D[index_1][index_1] = 1.0;
1309  D[index_2][index_2] = -1.;
1310  meanR = U * D * V.t();
1311  }
1312 
1313  R = meanR;
1314 
1315  vpTranslationVector t(meanT);
1317  return mean;
1318 }
1319 
1320 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1321 
1328 void vpHomogeneousMatrix::setIdentity() { eye(); }
1329 
1330 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1331 
1332 #ifdef VISP_HAVE_NLOHMANN_JSON
1333 const std::string vpHomogeneousMatrix::jsonTypeName = "vpHomogeneousMatrix";
1334 #include <visp3/core/vpJsonParsing.h>
1335 void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j) const
1336 {
1337  const vpArray2D<double> *asArray = (vpArray2D<double>*) this;
1338  to_json(j, *asArray);
1340 }
1341 
1342 void vpHomogeneousMatrix::parse_json(const nlohmann::json &j)
1343 {
1344 #ifdef ENABLE_VISP_NAMESPACE
1345  using namespace VISP_NAMESPACE_NAME;
1346 #endif
1347  vpArray2D<double> *asArray = (vpArray2D<double>*) this;
1348  if (j.is_object() && j.contains("type")) { // Specific conversions
1349  const bool converted = convertFromTypeAndBuildFrom<vpHomogeneousMatrix, vpPoseVector>(j, *this);
1350  if (!converted) {
1351  from_json(j, *asArray);
1352  }
1353  }
1354  else { // Generic 2D array conversion
1355  from_json(j, *asArray);
1356  }
1357 
1358  if ((getCols() != 4) && (getRows() != 4)) {
1359  throw vpException(vpException::badValue, "From JSON, tried to read something that is not a 4x4 matrix");
1360  }
1361  if (!isAnHomogeneousMatrix()) {
1362  throw vpException(vpException::badValue, "From JSON read a non homogeneous matrix into a vpHomogeneousMatrix");
1363  }
1364 }
1365 #endif
1366 END_VISP_NAMESPACE
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1105
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
vpArray2D< double > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1166
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ ioError
I/O error.
Definition: vpException.h:67
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
void load(std::ifstream &f)
friend void from_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
static const std::string jsonTypeName
void print() const
Print the matrix as a pose vector .
vpRotationMatrix getRotationMatrix() const
bool isAnHomogeneousMatrix(double threshold=1e-6) const
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
vpHomogeneousMatrix & operator*=(const vpHomogeneousMatrix &M)
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpTranslationVector getTranslationVector() const
void convert(std::vector< float > &M)
void extract(vpRotationMatrix &R) const
vpColVector getCol(unsigned int j) const
vpHomogeneousMatrix & operator<<(double val)
void insert(const vpRotationMatrix &R)
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
void save(std::ofstream &f) const
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
friend void to_json(nlohmann::json &j, const vpHomogeneousMatrix &T)
vpHomogeneousMatrix & operator,(double val)
static bool isNaN(double value)
Definition: vpMath.cpp:92
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:450
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:1641
vpMatrix t() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:452
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:404
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:457
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:446
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:408
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:448
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:459
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:450
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:455
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:402
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
vpRowVector t() const
Implementation of a rotation vector as quaternion angle minimal representation.
vpQuaternionVector & buildFrom(const double &qx, const double &qy, const double &qz, const double &qw)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector & buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.