Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
vpColVector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Provide some simple operation on column vectors.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <assert.h>
46 #include <cmath> // std::fabs
47 #include <limits> // numeric_limits
48 #include <math.h>
49 #include <sstream>
50 #include <stdio.h>
51 #include <stdlib.h>
52 #include <string.h>
53 
54 #include <visp3/core/vpCPUFeatures.h>
55 #include <visp3/core/vpColVector.h>
56 #include <visp3/core/vpDebug.h>
57 #include <visp3/core/vpException.h>
58 #include <visp3/core/vpMath.h>
59 #include <visp3/core/vpRotationVector.h>
60 
61 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
62 #include <emmintrin.h>
63 #define VISP_HAVE_SSE2 1
64 #endif
65 
68 {
69  if (getRows() != v.getRows()) {
70  throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
71  v.getRows()));
72  }
74 
75  for (unsigned int i = 0; i < rowNum; i++)
76  r[i] = (*this)[i] + v[i];
77  return r;
78 }
101 {
102  if (getRows() != 3) {
103  throw(vpException(vpException::dimensionError, "Cannot add %d-dimension column vector to a translation vector",
104  getRows()));
105  }
107 
108  for (unsigned int i = 0; i < 3; i++)
109  s[i] = (*this)[i] + t[i];
110 
111  return s;
112 }
113 
116 {
117  if (getRows() != v.getRows()) {
118  throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
119  v.getRows()));
120  }
121 
122  for (unsigned int i = 0; i < rowNum; i++)
123  (*this)[i] += v[i];
124  return (*this);
125 }
128 {
129  if (getRows() != v.getRows()) {
130  throw(vpException(vpException::dimensionError, "Cannot substract (%dx1) column vector to (%dx1) column vector",
131  getRows(), v.getRows()));
132  }
133 
134  for (unsigned int i = 0; i < rowNum; i++)
135  (*this)[i] -= v[i];
136  return (*this);
137 }
138 
146 double vpColVector::operator*(const vpColVector &v) const
147 {
148  if (size() != v.size()) {
150  "Cannot compute the dot product between column vectors "
151  "with different dimensions (%d) and (%d)",
152  size(), v.size()));
153  }
154  double r = 0;
155 
156  for (unsigned int i = 0; i < rowNum; i++)
157  r += (*this)[i] * v[i];
158  return r;
159 }
160 
171 {
172  vpMatrix M(rowNum, v.getCols());
173  for (unsigned int i = 0; i < rowNum; i++) {
174  for (unsigned int j = 0; j < v.getCols(); j++) {
175  M[i][j] = (*this)[i] * v[j];
176  }
177  }
178  return M;
179 }
180 
183 {
184  if (getRows() != m.getRows()) {
186  "Bad size during vpColVector (%dx1) and vpColVector "
187  "(%dx1) substraction",
188  getRows(), m.getRows()));
189  }
190  vpColVector v(rowNum);
191 
192  for (unsigned int i = 0; i < rowNum; i++)
193  v[i] = (*this)[i] - m[i];
194  return v;
195 }
196 
210 vpColVector::vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) : vpArray2D<double>(nrows, 1)
211 {
212  init(v, r, nrows);
213 }
214 
252 void vpColVector::init(const vpColVector &v, unsigned int r, unsigned int nrows)
253 {
254  unsigned int rnrows = r + nrows;
255 
256  if (rnrows > v.getRows())
257  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpColVector", rnrows,
258  v.getRows()));
259  resize(nrows, false);
260 
261  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
262  return; // Nothing to do
263  for (unsigned int i = r; i < rnrows; i++)
264  (*this)[i - r] = v[i];
265 }
266 
268 {
269  for (unsigned int i = 0; i < v.size(); i++)
270  (*this)[i] = v[i];
271 }
272 
274 {
275  for (unsigned int i = 0; i < p.size(); i++)
276  (*this)[i] = p[i];
277 }
278 
280 {
281  for (unsigned int i = 0; i < v.size(); i++)
282  (*this)[i] = v[i];
283 }
284 
286 vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D<double>(M.getRows(), 1)
287 {
288  for (unsigned int i = 0; i < M.getCols(); i++)
289  (*this)[i] = M[i][j];
290 }
291 
299 {
300  if (M.getCols() != 1) {
301  throw(vpException(vpException::dimensionError, "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
302  M.getRows(), M.getRows(), M.getCols()));
303  }
304 
305  for (unsigned int i = 0; i < M.getRows(); i++)
306  (*this)[i] = M[i][0];
307 }
308 
312 vpColVector::vpColVector(const std::vector<double> &v) : vpArray2D<double>((unsigned int)v.size(), 1)
313 {
314  for (unsigned int i = 0; i < v.size(); i++)
315  (*this)[i] = v[i];
316 }
320 vpColVector::vpColVector(const std::vector<float> &v) : vpArray2D<double>((unsigned int)v.size(), 1)
321 {
322  for (unsigned int i = 0; i < v.size(); i++)
323  (*this)[i] = (double)(v[i]);
324 }
325 
326 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
327 
331 {
332  rowNum = v.rowNum;
333  colNum = v.colNum;
334  rowPtrs = v.rowPtrs;
335  dsize = v.dsize;
336  data = v.data;
337 
338  v.rowNum = 0;
339  v.colNum = 0;
340  v.rowPtrs = NULL;
341  v.dsize = 0;
342  v.data = NULL;
343 }
344 #endif
345 
357 {
358  vpColVector A;
359  A.resize(rowNum, false);
360 
361  double *vd = A.data;
362  double *d = data;
363 
364  for (unsigned int i = 0; i < rowNum; i++)
365  *(vd++) = -(*d++);
366 
367  return A;
368 }
369 
391 {
392  vpColVector v(rowNum);
393 
394  double *vd = v.data;
395  double *d = data;
396 
397  for (unsigned int i = 0; i < rowNum; i++)
398  *(vd++) = (*d++) * x;
399  return v;
400 }
401 
421 {
422  for (unsigned int i = 0; i < rowNum; i++)
423  (*this)[i] *= x;
424  return (*this);
425 }
426 
445 {
446  for (unsigned int i = 0; i < rowNum; i++)
447  (*this)[i] /= x;
448  return (*this);
449 }
450 
471 {
472  vpColVector v(rowNum);
473 
474  double *vd = v.data;
475  double *d = data;
476 
477  for (unsigned int i = 0; i < rowNum; i++)
478  *(vd++) = (*d++) / x;
479  return v;
480 }
481 
488 {
489  if (M.getCols() != 1) {
490  throw(vpException(vpException::dimensionError, "Cannot transform a (%dx%d) matrix into a column vector",
491  M.getRows(), M.getCols()));
492  }
493 
494  resize(M.getRows(), false);
495  memcpy(data, M.data, rowNum * sizeof(double));
496 
497  return (*this);
498 }
499 
503 vpColVector &vpColVector::operator=(const std::vector<double> &v)
504 {
505  resize((unsigned int)v.size(), false);
506  for (unsigned int i = 0; i < v.size(); i++)
507  (*this)[i] = v[i];
508  return *this;
509 }
513 vpColVector &vpColVector::operator=(const std::vector<float> &v)
514 {
515  resize((unsigned int)v.size(), false);
516  for (unsigned int i = 0; i < v.size(); i++)
517  (*this)[i] = (float)v[i];
518  return *this;
519 }
520 
522 {
523  unsigned int k = v.rowNum;
524  if (rowNum != k) {
525  resize(k, false);
526  }
527 
528  memcpy(data, v.data, rowNum * sizeof(double));
529  return *this;
530 }
531 
536 {
537  unsigned int k = tv.getRows();
538  if (rowNum != k) {
539  resize(k, false);
540  }
541 
542  memcpy(data, tv.data, rowNum * sizeof(double));
543  return *this;
544 }
549 {
550  unsigned int k = rv.getRows();
551  if (rowNum != k) {
552  resize(k, false);
553  }
554 
555  memcpy(data, rv.data, rowNum * sizeof(double));
556  return *this;
557 }
562 {
563  unsigned int k = p.getRows();
564  if (rowNum != k) {
565  resize(k, false);
566  }
567 
568  memcpy(data, p.data, rowNum * sizeof(double));
569  return *this;
570 }
571 
593 {
594  *this = v;
595  return *this;
596 }
597 
623 {
624  for (unsigned int i = 0; i < rowNum; i++) {
625  for (unsigned int j = 0; j < colNum; j++) {
626  rowPtrs[i][j] = *x++;
627  }
628  }
629  return *this;
630 }
631 
651 {
652  resize(1, false);
653  data[0] = val;
654  return *this;
655 }
656 
676 {
677  resize(rowNum + 1, false);
678  data[rowNum - 1] = val;
679  return *this;
680 }
681 
684 {
685  double *d = data;
686 
687  for (unsigned int i = 0; i < rowNum; i++)
688  *(d++) = x;
689  return *this;
690 }
691 
696 std::vector<double> vpColVector::toStdVector()
697 {
698  std::vector<double> v(this->size());
699 
700  for (unsigned int i = 0; i < this->size(); i++)
701  v[i] = data[i];
702  return v;
703 }
704 
705 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
706 
710 {
711  if (this != &other) {
712  free(data);
713  free(rowPtrs);
714 
715  rowNum = other.rowNum;
716  colNum = other.colNum;
717  rowPtrs = other.rowPtrs;
718  dsize = other.dsize;
719  data = other.data;
720 
721  other.rowNum = 0;
722  other.colNum = 0;
723  other.rowPtrs = NULL;
724  other.dsize = 0;
725  other.data = NULL;
726  }
727 
728  return *this;
729 }
730 
754 vpColVector& vpColVector::operator=(const std::initializer_list<double> &list)
755 {
756  resize(static_cast<unsigned int>(list.size()), false);
757  std::copy(list.begin(), list.end(), data);
758  return *this;
759 }
760 #endif
761 
762 bool vpColVector::operator==(const vpColVector &v) const {
763  if (rowNum != v.rowNum ||
764  colNum != v.colNum /* should not happen */)
765  return false;
766 
767  for (unsigned int i = 0; i < rowNum; i++) {
768  if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon()))
769  return false;
770  }
771 
772  return true;
773 }
774 
775 bool vpColVector::operator!=(const vpColVector &v) const {
776  return !(*this == v);
777 }
778 
783 {
784  vpRowVector v(rowNum);
785  memcpy(v.data, data, rowNum * sizeof(double));
786  return v;
787 }
788 
793 vpRowVector vpColVector::transpose() const { return t(); }
794 
799 void vpColVector::transpose(vpRowVector &v) const { v = t(); }
800 
805 vpColVector operator*(const double &x, const vpColVector &v)
806 {
807  vpColVector vout;
808  vout = v * x;
809  return vout;
810 }
811 
819 double vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
820 {
821  if (a.data == NULL) {
822  throw(vpException(vpException::fatalError, "Cannot compute the dot product: first vector empty"));
823  }
824  if (b.data == NULL) {
825  throw(vpException(vpException::fatalError, "Cannot compute the dot product: second vector empty"));
826  }
827  if (a.size() != b.size()) {
829  "Cannot compute the dot product between column vectors "
830  "with different dimensions (%d) and (%d)",
831  a.size(), b.size()));
832  }
833 
834  double *ad = a.data;
835  double *bd = b.data;
836 
837  double c = 0;
838  for (unsigned int i = 0; i < a.getRows(); i++)
839  c += *(ad++) * *(bd++);
840  // vpMatrix c = (a.t() * b);
841  // return c[0][0];
842  return c;
843 }
844 
853 {
854  x = x / sqrt(x.sumSquare());
855 
856  return x;
857 }
858 
867 {
868 
869  double sum_square = sumSquare();
870 
871  // if (sum != 0.0)
872  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
873  *this /= sqrt(sum_square);
874 
875  // If sum = 0, we have a nul vector. So we return just.
876  return *this;
877 }
878 
908 {
909  if (v.data == NULL) {
910  throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
911  }
912  vpColVector tab;
913  tab = v;
914  unsigned int nb_permutation = 1;
915  unsigned int i = 0;
916  while (nb_permutation != 0) {
917  nb_permutation = 0;
918  for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) {
919  if ((tab[j] > tab[j - 1])) {
920  double tmp = tab[j];
921  tab[j] = tab[j - 1];
922  tab[j - 1] = tmp;
923  nb_permutation++;
924  }
925  }
926  i++;
927  }
928 
929  return tab;
930 }
931 
960 {
961  if (v.data == NULL) {
962  throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
963  }
964  vpColVector tab;
965  tab = v;
966  unsigned int nb_permutation = 1;
967  unsigned int i = 0;
968  while (nb_permutation != 0) {
969  nb_permutation = 0;
970  for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) {
971  if ((tab[j] < tab[j - 1])) {
972  double tmp = tab[j];
973  tab[j] = tab[j - 1];
974  tab[j - 1] = tmp;
975  nb_permutation++;
976  }
977  }
978  i++;
979  }
980 
981  return tab;
982 }
983 
1000 void vpColVector::stack(double d)
1001 {
1002  this->resize(rowNum + 1, false);
1003  (*this)[rowNum - 1] = d;
1004 }
1005 
1025 void vpColVector::stack(const vpColVector &v) { *this = vpColVector::stack(*this, v); }
1026 
1046 {
1047  vpColVector C;
1048  vpColVector::stack(A, B, C);
1049  return C;
1050 }
1051 
1071 {
1072  unsigned int nrA = A.getRows();
1073  unsigned int nrB = B.getRows();
1074 
1075  if (nrA == 0 && nrB == 0) {
1076  C.resize(0);
1077  return;
1078  }
1079 
1080  if (nrB == 0) {
1081  C = A;
1082  return;
1083  }
1084 
1085  if (nrA == 0) {
1086  C = B;
1087  return;
1088  }
1089 
1090  // General case
1091  C.resize(nrA + nrB, false);
1092 
1093  for (unsigned int i = 0; i < nrA; i++)
1094  C[i] = A[i];
1095 
1096  for (unsigned int i = 0; i < nrB; i++)
1097  C[nrA + i] = B[i];
1098 }
1099 
1104 {
1105  if (v.data == NULL || v.size() == 0) {
1106  throw(vpException(vpException::dimensionError, "Cannot compute column vector mean: vector empty"));
1107  }
1108 
1109  // Use directly sum() function
1110  double mean = v.sum();
1111 
1112  // Old code used
1113  // double *vd = v.data;
1114  // for (unsigned int i=0 ; i < v.getRows() ; i++)
1115  // mean += *(vd++);
1116 
1117  return mean / v.getRows();
1118 }
1119 
1124 {
1125  if (v.data == NULL || v.size() == 0) {
1126  throw(vpException(vpException::dimensionError, "Cannot compute column vector median: vector empty"));
1127  }
1128 
1129  std::vector<double> vectorOfDoubles(v.data, v.data + v.rowNum);
1130 
1131  return vpMath::getMedian(vectorOfDoubles);
1132 }
1133 
1137 double vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection)
1138 {
1139  if (v.data == NULL || v.size() == 0) {
1140  throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty"));
1141  }
1142 
1143  double mean_value = mean(v);
1144  double sum_squared_diff = 0.0;
1145  unsigned int i = 0;
1146 
1147 #if VISP_HAVE_SSE2
1148  if (vpCPUFeatures::checkSSE2()) {
1149  __m128d v_sub, v_mul, v_sum = _mm_setzero_pd();
1150  __m128d v_mean = _mm_set1_pd(mean_value);
1151 
1152  if (v.getRows() >= 4) {
1153  for (; i <= v.getRows() - 4; i += 4) {
1154  v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i), v_mean);
1155  v_mul = _mm_mul_pd(v_sub, v_sub);
1156  v_sum = _mm_add_pd(v_mul, v_sum);
1157 
1158  v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i + 2), v_mean);
1159  v_mul = _mm_mul_pd(v_sub, v_sub);
1160  v_sum = _mm_add_pd(v_mul, v_sum);
1161  }
1162  }
1163 
1164  double res[2];
1165  _mm_storeu_pd(res, v_sum);
1166 
1167  sum_squared_diff = res[0] + res[1];
1168  }
1169 // Old code used before SSE
1170 //#else
1171 // for(unsigned int i = 0; i < v.size(); i++) {
1172 // sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
1173 // }
1174 #endif
1175 
1176  for (; i < v.getRows(); i++) {
1177  sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
1178  }
1179 
1180  double divisor = (double)v.size();
1181  if (useBesselCorrection && v.size() > 1) {
1182  divisor = divisor - 1;
1183  }
1184 
1185  return std::sqrt(sum_squared_diff / divisor);
1186 }
1187 
1203 {
1204  vpMatrix M;
1205  if (v.getRows() != 3) {
1206  throw(vpException(vpException::dimensionError, "Cannot compute skew vector of a non 3-dimention vector (%d)",
1207  v.getRows()));
1208  }
1209 
1210  M.resize(3, 3, false, false);
1211  M[0][0] = 0;
1212  M[0][1] = -v[2];
1213  M[0][2] = v[1];
1214  M[1][0] = v[2];
1215  M[1][1] = 0;
1216  M[1][2] = -v[0];
1217  M[2][0] = -v[1];
1218  M[2][1] = v[0];
1219  M[2][2] = 0;
1220 
1221  return M;
1222 }
1223 
1235 {
1236  if (a.getRows() != 3 || b.getRows() != 3) {
1238  "Cannot compute the cross product between column "
1239  "vector with dimension %d and %d",
1240  a.getRows(), b.getRows()));
1241  }
1242 
1243  return vpColVector::skew(a) * b;
1244 }
1245 
1254 vpMatrix vpColVector::reshape(unsigned int nrows, unsigned int ncols)
1255 {
1256  vpMatrix M(nrows, ncols);
1257  reshape(M, nrows, ncols);
1258  return M;
1259 }
1260 
1316 void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
1317 {
1318  if (dsize != nrows * ncols) {
1319  throw(vpException(vpException::dimensionError, "Cannot reshape (%dx1) column vector in (%dx%d) matrix", rowNum,
1320  M.getRows(), M.getCols()));
1321  }
1322  if ((M.getRows() != nrows) || (M.getCols() != ncols))
1323  M.resize(nrows, ncols, false, false);
1324 
1325  for (unsigned int j = 0; j < ncols; j++)
1326  for (unsigned int i = 0; i < nrows; i++)
1327  M[i][j] = data[j * nrows + i];
1328 }
1329 
1362 void vpColVector::insert(unsigned int i, const vpColVector &v)
1363 {
1364  if (i + v.size() > this->size())
1365  throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
1366 
1367  if (data != NULL && v.data != NULL && v.rowNum > 0) {
1368  memcpy(data + i, v.data, sizeof(double) * v.rowNum);
1369  }
1370 }
1371 
1391 int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) const
1392 {
1393  typedef std::string::size_type size_type;
1394 
1395  unsigned int m = getRows();
1396  unsigned int n = 1;
1397 
1398  std::vector<std::string> values(m * n);
1399  std::ostringstream oss;
1400  std::ostringstream ossFixed;
1401  std::ios_base::fmtflags original_flags = oss.flags();
1402 
1403  // ossFixed <<std::fixed;
1404  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
1405 
1406  size_type maxBefore = 0; // the length of the integral part
1407  size_type maxAfter = 0; // number of decimals plus
1408  // one place for the decimal point
1409  for (unsigned int i = 0; i < m; ++i) {
1410  oss.str("");
1411  oss << (*this)[i];
1412  if (oss.str().find("e") != std::string::npos) {
1413  ossFixed.str("");
1414  ossFixed << (*this)[i];
1415  oss.str(ossFixed.str());
1416  }
1417 
1418  values[i] = oss.str();
1419  size_type thislen = values[i].size();
1420  size_type p = values[i].find('.');
1421 
1422  if (p == std::string::npos) {
1423  maxBefore = vpMath::maximum(maxBefore, thislen);
1424  // maxAfter remains the same
1425  } else {
1426  maxBefore = vpMath::maximum(maxBefore, p);
1427  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
1428  }
1429  }
1430 
1431  size_type totalLength = length;
1432  // increase totalLength according to maxBefore
1433  totalLength = vpMath::maximum(totalLength, maxBefore);
1434  // decrease maxAfter according to totalLength
1435  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
1436  if (maxAfter == 1)
1437  maxAfter = 0;
1438 
1439  // the following line is useful for debugging
1440  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
1441 
1442  if (intro)
1443  s << intro;
1444  s << "[" << m << "," << n << "]=\n";
1445 
1446  for (unsigned int i = 0; i < m; i++) {
1447  s << " ";
1448  size_type p = values[i].find('.');
1449  s.setf(std::ios::right, std::ios::adjustfield);
1450  s.width((std::streamsize)maxBefore);
1451  s << values[i].substr(0, p).c_str();
1452 
1453  if (maxAfter > 0) {
1454  s.setf(std::ios::left, std::ios::adjustfield);
1455  if (p != std::string::npos) {
1456  s.width((std::streamsize)maxAfter);
1457  s << values[i].substr(p, maxAfter).c_str();
1458  } else {
1459  assert(maxAfter > 1);
1460  s.width((std::streamsize)maxAfter);
1461  s << ".0";
1462  }
1463  }
1464 
1465  s << ' ';
1466 
1467  s << std::endl;
1468  }
1469 
1470  s.flags(original_flags); // restore s to standard state
1471 
1472  return (int)(maxBefore + maxAfter);
1473 }
1474 
1480 double vpColVector::sum() const
1481 {
1482  double sum = 0.0;
1483  unsigned int i = 0;
1484 
1485 #if VISP_HAVE_SSE2
1486  if (vpCPUFeatures::checkSSE2()) {
1487  __m128d v_sum1 = _mm_setzero_pd(), v_sum2 = _mm_setzero_pd(), v_sum;
1488 
1489  if (rowNum >= 4) {
1490  for (; i <= rowNum - 4; i += 4) {
1491  v_sum1 = _mm_add_pd(_mm_loadu_pd(data + i), v_sum1);
1492  v_sum2 = _mm_add_pd(_mm_loadu_pd(data + i + 2), v_sum2);
1493  }
1494  }
1495 
1496  v_sum = _mm_add_pd(v_sum1, v_sum2);
1497 
1498  double res[2];
1499  _mm_storeu_pd(res, v_sum);
1500 
1501  sum = res[0] + res[1];
1502  }
1503 // Old code used before SSE
1504 //#else
1505 // for (unsigned int i=0;i<rowNum;i++) {
1506 // sum += rowPtrs[i][0];
1507 // }
1508 #endif
1509 
1510  for (; i < rowNum; i++) {
1511  sum += (*this)[i];
1512  }
1513 
1514  return sum;
1515 }
1516 
1524 {
1525  double sum_square = 0.0;
1526  unsigned int i = 0;
1527 
1528 #if VISP_HAVE_SSE2
1529  if (vpCPUFeatures::checkSSE2()) {
1530  __m128d v_mul1, v_mul2;
1531  __m128d v_sum = _mm_setzero_pd();
1532 
1533  if (rowNum >= 4) {
1534  for (; i <= rowNum - 4; i += 4) {
1535  v_mul1 = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(data + i));
1536  v_mul2 = _mm_mul_pd(_mm_loadu_pd(data + i + 2), _mm_loadu_pd(data + i + 2));
1537 
1538  v_sum = _mm_add_pd(v_mul1, v_sum);
1539  v_sum = _mm_add_pd(v_mul2, v_sum);
1540  }
1541  }
1542 
1543  double res[2];
1544  _mm_storeu_pd(res, v_sum);
1545 
1546  sum_square = res[0] + res[1];
1547  }
1548 // Old code used before SSE
1549 //#else
1550 // for (unsigned int i=0;i<rowNum;i++) {
1551 // double x=rowPtrs[i][0];
1552 // sum_square += x*x;
1553 // }
1554 #endif
1555 
1556  for (; i < rowNum; i++) {
1557  sum_square += (*this)[i] * (*this)[i];
1558  }
1559 
1560  return sum_square;
1561 }
1562 
1574 {
1575  return frobeniusNorm();
1576 }
1577 
1587 {
1588  double norm = sumSquare();
1589 
1590  return sqrt(norm);
1591 }
1592 
1600 {
1601  if (v.getRows() != rowNum || v.getCols() != colNum) {
1602  throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
1603  }
1604 
1605  vpColVector out;
1606  out.resize(rowNum, false);
1607 
1608  unsigned int i = 0;
1609 
1610 #if VISP_HAVE_SSE2
1611  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
1612  for (; i <= dsize - 2; i += 2) {
1613  __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(v.data + i));
1614  _mm_storeu_pd(out.data + i, vout);
1615  }
1616  }
1617 #endif
1618 
1619  for (; i < dsize; i++) {
1620  out.data[i] = data[i] * v.data[i];
1621  }
1622 
1623  return out;
1624 }
1625 
1638 {
1639  double norm = 0.0;
1640  for (unsigned int i = 0; i < rowNum; i++) {
1641  double x = fabs((*this)[i]);
1642  if (x > norm) {
1643  norm = x;
1644  }
1645  }
1646  return norm;
1647 }
1648 
1677 std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
1678 {
1679  os << "vpColVector " << matrixName << " (" << this->getRows() << "); " << std::endl;
1680 
1681  for (unsigned int i = 0; i < this->getRows(); ++i) {
1682 
1683  if (!octet) {
1684  os << matrixName << "[" << i << "] = " << (*this)[i] << "; " << std::endl;
1685  } else {
1686  for (unsigned int k = 0; k < sizeof(double); ++k) {
1687  os << "((unsigned char*)&(" << matrixName << "[" << i << "]) )[" << k << "] = 0x" << std::hex
1688  << (unsigned int)((unsigned char *)&((*this)[i]))[k] << "; " << std::endl;
1689  }
1690  }
1691  }
1692  std::cout << std::endl;
1693  return os;
1694 };
1695 
1722 std::ostream &vpColVector::csvPrint(std::ostream &os) const
1723 {
1724  for (unsigned int i = 0; i < this->getRows(); ++i) {
1725  os << (*this)[i];
1726 
1727  os << std::endl;
1728  }
1729  return os;
1730 };
1731 
1757 std::ostream &vpColVector::maplePrint(std::ostream &os) const
1758 {
1759  os << "([ " << std::endl;
1760  for (unsigned int i = 0; i < this->getRows(); ++i) {
1761  os << "[";
1762  os << (*this)[i] << ", ";
1763  os << "]," << std::endl;
1764  }
1765  os << "])" << std::endl;
1766  return os;
1767 };
1768 
1805 std::ostream &vpColVector::matlabPrint(std::ostream &os) const
1806 {
1807  os << "[ ";
1808  for (unsigned int i = 0; i < this->getRows(); ++i) {
1809  os << (*this)[i] << ", ";
1810  if (this->getRows() != i + 1) {
1811  os << ";" << std::endl;
1812  } else {
1813  os << "]" << std::endl;
1814  }
1815  }
1816  return os;
1817 };
1818 
1819 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1820 
1834 void vpColVector::insert(const vpColVector &v, const unsigned int r, const unsigned int c)
1835 {
1836  (void)c;
1837  insert(r, v);
1838 }
1839 #endif // defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
bool operator==(const vpColVector &v) const
Comparison operator.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
Implementation of a generic rotation vector.
double frobeniusNorm() const
vp_deprecated void init()
Definition: vpColVector.h:377
static vpColVector invSort(const vpColVector &v)
static vpColVector sort(const vpColVector &v)
static double stdev(const vpColVector &v, const bool useBesselCorrection=false)
vp_deprecated double euclideanNorm() const
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
std::ostream & matlabPrint(std::ostream &os) const
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
vpColVector operator*(const double &x, const vpColVector &v)
vpColVector operator/(const double x) const
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
vpColVector & operator,(double val)
vpColVector operator+(const vpColVector &v) const
Operator that allows to add two column vectors.
Definition: vpColVector.cpp:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
std::ostream & csvPrint(std::ostream &os) const
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
vpColVector & operator*=(double x)
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getCols() const
Definition: vpArray2D.h:279
vpColVector operator-() const
static double median(const vpColVector &v)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
vpColVector & operator/=(double x)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
bool operator!=(const vpColVector &v) const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double infinityNorm() const
vpColVector & normalize()
vpColVector hadamard(const vpColVector &v) const
std::ostream & maplePrint(std::ostream &os) const
VISP_EXPORT bool checkSSE2()
vpRowVector t() const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
vpColVector & operator<<(const vpColVector &v)
static double mean(const vpColVector &v)
unsigned int getRows() const
Definition: vpArray2D.h:289
vpColVector & operator-=(vpColVector v)
Operator that allows to substract two column vectors.
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
void insert(unsigned int i, const vpColVector &v)
double sumSquare() const
std::vector< double > toStdVector()
vpColVector & operator=(const vpColVector &v)
Copy operator. Allow operation such as A = v.
vpColVector & operator+=(vpColVector v)
Operator that allows to add two column vectors.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static double dotProd(const vpColVector &a, const vpColVector &b)
void stack(double d)
vpRowVector transpose() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpColVector()
Basic constructor that creates an empty 0-size column vector.
Definition: vpColVector.h:136
static vpMatrix skew(const vpColVector &v)
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double operator*(const vpColVector &x) const
Class that consider the case of a translation vector.
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
double sum() const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310