Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
vpColVector.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  * Provide some simple operation on column vectors.
32  */
33 
40 #include <assert.h>
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 #include <math.h>
44 #include <sstream>
45 #include <stdio.h>
46 #include <stdlib.h>
47 #include <string.h>
48 
49 #include <visp3/core/vpCPUFeatures.h>
50 #include <visp3/core/vpColVector.h>
51 #include <visp3/core/vpException.h>
52 #include <visp3/core/vpMath.h>
53 #include <visp3/core/vpRotationVector.h>
54 
55 #if defined(VISP_HAVE_SIMDLIB)
56 #include <Simd/SimdLib.h>
57 #endif
58 
59 BEGIN_VISP_NAMESPACE
61 {
62  if (getRows() != v.getRows()) {
63  throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
64  v.getRows()));
65  }
67 
68  for (unsigned int i = 0; i < rowNum; ++i) {
69  r[i] = (*this)[i] + v[i];
70  }
71  return r;
72 }
73 
75 {
76  const unsigned int val_3 = 3;
77  if (getRows() != val_3) {
78  throw(vpException(vpException::dimensionError, "Cannot add %d-dimension column vector to a translation vector",
79  getRows()));
80  }
82 
83  for (unsigned int i = 0; i < val_3; ++i) {
84  s[i] = (*this)[i] + t[i];
85  }
86 
87  return s;
88 }
89 
91 {
92  if (getRows() != v.getRows()) {
93  throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
94  v.getRows()));
95  }
96 
97  for (unsigned int i = 0; i < rowNum; ++i) {
98  (*this)[i] += v[i];
99  }
100  return (*this);
101 }
102 
104 {
105  if (getRows() != v.getRows()) {
106  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx1) column vector to (%dx1) column vector",
107  getRows(), v.getRows()));
108  }
109 
110  for (unsigned int i = 0; i < rowNum; ++i) {
111  (*this)[i] -= v[i];
112  }
113  return (*this);
114 }
115 
116 double vpColVector::operator*(const vpColVector &v) const
117 {
118  if (size() != v.size()) {
120  "Cannot compute the dot product between column vectors "
121  "with different dimensions (%d) and (%d)",
122  size(), v.size()));
123  }
124  double r = 0;
125 
126  for (unsigned int i = 0; i < rowNum; ++i) {
127  r += (*this)[i] * v[i];
128  }
129  return r;
130 }
131 
133 {
134  vpMatrix M(rowNum, v.getCols());
135  unsigned int v_cols = v.getCols();
136  for (unsigned int i = 0; i < rowNum; ++i) {
137  for (unsigned int j = 0; j < v_cols; ++j) {
138  M[i][j] = (*this)[i] * v[j];
139  }
140  }
141  return M;
142 }
143 
145 {
146  if (M.getRows() != 1) {
148  "Bad size during vpColVector (%dx1) and vpMatrix (%dx%d) multiplication",
149  getRows(), M.getRows(), M.getCols()));
150  }
151  vpMatrix R(rowNum, M.getCols());
152  unsigned int M_cols = M.getCols();
153  for (unsigned int i = 0; i < rowNum; ++i) {
154  for (unsigned int j = 0; j < M_cols; ++j) {
155  R[i][j] = (*this)[i] * M[0][j];
156  }
157  }
158  return R;
159 }
160 
162 {
163  if (getRows() != m.getRows()) {
165  "Bad size during vpColVector (%dx1) and vpColVector "
166  "(%dx1) subtraction",
167  getRows(), m.getRows()));
168  }
169  vpColVector v(rowNum);
170 
171  for (unsigned int i = 0; i < rowNum; ++i) {
172  v[i] = (*this)[i] - m[i];
173  }
174  return v;
175 }
176 
177 vpColVector::vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) : vpArray2D<double>(nrows, 1)
178 {
179  init(v, r, nrows);
180 }
181 
182 void vpColVector::init(const vpColVector &v, unsigned int r, unsigned int nrows)
183 {
184  unsigned int rnrows = r + nrows;
185 
186  if (rnrows > v.getRows()) {
187  throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpColVector", rnrows,
188  v.getRows()));
189  }
190  resize(nrows, false);
191 
192  if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced
193  return; // Nothing to do
194  }
195  for (unsigned int i = r; i < rnrows; ++i) {
196  (*this)[i - r] = v[i];
197  }
198 }
199 
200 vpColVector::vpColVector(const vpRotationVector &v) : vpArray2D<double>(v.size(), 1)
201 {
202  unsigned int v_size = v.size();
203  for (unsigned int i = 0; i < v_size; ++i) {
204  (*this)[i] = v[i];
205  }
206 }
207 
208 vpColVector::vpColVector(const vpPoseVector &p) : vpArray2D<double>(p.size(), 1)
209 {
210  unsigned int p_size = p.size();
211  for (unsigned int i = 0; i < p_size; ++i) {
212  (*this)[i] = p[i];
213  }
214 }
215 
217 {
218  unsigned int v_size = v.size();
219  for (unsigned int i = 0; i < v_size; ++i) {
220  (*this)[i] = v[i];
221  }
222 }
223 
224 vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D<double>(M.getRows(), 1)
225 {
226  unsigned int m_cols = M.getCols();
227  for (unsigned int i = 0; i < m_cols; ++i) {
228  (*this)[i] = M[i][j];
229  }
230 }
231 
232 vpColVector::vpColVector(const vpMatrix &M) : vpArray2D<double>(M.getRows(), 1)
233 {
234  if (M.getCols() != 1) {
235  throw(vpException(vpException::dimensionError, "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
236  M.getRows(), M.getRows(), M.getCols()));
237  }
238 
239  unsigned int m_rows = M.getRows();
240  for (unsigned int i = 0; i < m_rows; ++i) {
241  (*this)[i] = M[i][0];
242  }
243 }
244 
245 vpColVector::vpColVector(const std::vector<double> &v) : vpArray2D<double>(static_cast<unsigned int>(v.size()), 1)
246 {
247  unsigned int v_size = static_cast<unsigned int>(v.size());
248  for (unsigned int i = 0; i < v_size; ++i) {
249  (*this)[i] = v[i];
250  }
251 }
252 
253 vpColVector::vpColVector(const std::vector<float> &v) : vpArray2D<double>(static_cast<unsigned int>(v.size()), 1)
254 {
255  unsigned int v_size = static_cast<unsigned int>(v.size());
256  for (unsigned int i = 0; i < v_size; ++i) {
257  (*this)[i] = static_cast<double>(v[i]);
258  }
259 }
260 
261 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
263 {
264  rowNum = v.rowNum;
265  colNum = v.colNum;
266  rowPtrs = v.rowPtrs;
267  dsize = v.dsize;
268  data = v.data;
269 
270  v.rowNum = 0;
271  v.colNum = 0;
272  v.rowPtrs = nullptr;
273  v.dsize = 0;
274  v.data = nullptr;
275 }
276 #endif
277 
279 {
280  vpColVector A;
281  A.resize(rowNum, false);
282 
283  double *vd = A.data;
284  double *d = data;
285 
286  for (unsigned int i = 0; i < rowNum; ++i) {
287  // move the d++ increment/decrement into a dedicated expression-statement
288  *vd = -(*d);
289  ++vd;
290  ++d;
291  }
292 
293  return A;
294 }
295 
297 {
298  vpColVector v(rowNum);
299 
300  double *vd = v.data;
301  double *d = data;
302 
303  for (unsigned int i = 0; i < rowNum; ++i) {
304  // move the d++ increment/decrement into a dedicated expression-statement
305  *vd = (*d) * x;
306  ++vd;
307  ++d;
308  }
309  return v;
310 }
311 
313 {
314  for (unsigned int i = 0; i < rowNum; ++i) {
315  (*this)[i] *= x;
316  }
317  return (*this);
318 }
319 
321 {
322  for (unsigned int i = 0; i < rowNum; ++i) {
323  (*this)[i] /= x;
324  }
325  return (*this);
326 }
327 
329 {
330  vpColVector v(rowNum);
331 
332  double *vd = v.data;
333  double *d = data;
334 
335  for (unsigned int i = 0; i < rowNum; ++i) {
336  // move the d++ increment/decrement into a dedicated expression-statement
337  *vd = (*d) / x;
338  ++vd;
339  ++d;
340  }
341  return v;
342 }
343 
345 {
346  if (M.getCols() != 1) {
347  throw(vpException(vpException::dimensionError, "Cannot transform a (%dx%d) matrix into a column vector",
348  M.getRows(), M.getCols()));
349  }
350 
351  resize(M.getRows(), false);
352  memcpy(data, M.data, rowNum * sizeof(double));
353 
354  return (*this);
355 }
356 
357 vpColVector &vpColVector::operator=(const std::vector<double> &v)
358 {
359  unsigned int v_size = static_cast<unsigned int>(v.size());
360  resize(v_size, false);
361  for (unsigned int i = 0; i < v_size; ++i) {
362  (*this)[i] = v[i];
363  }
364  return *this;
365 }
366 
367 vpColVector &vpColVector::operator=(const std::vector<float> &v)
368 {
369  unsigned int v_size = static_cast<unsigned int>(v.size());
370  resize(v_size, false);
371  for (unsigned int i = 0; i < v_size; ++i) {
372  (*this)[i] = static_cast<float>(v[i]);
373  }
374  return *this;
375 }
376 
378 {
379  unsigned int k = v.rowNum;
380  if (rowNum != k) {
381  resize(k, false);
382  }
383 
384  memcpy(data, v.data, rowNum * sizeof(double));
385  return *this;
386 }
387 
389 {
390  unsigned int k = tv.getRows();
391  if (rowNum != k) {
392  resize(k, false);
393  }
394 
395  memcpy(data, tv.data, rowNum * sizeof(double));
396  return *this;
397 }
398 
400 {
401  unsigned int k = rv.getRows();
402  if (rowNum != k) {
403  resize(k, false);
404  }
405 
406  memcpy(data, rv.data, rowNum * sizeof(double));
407  return *this;
408 }
409 
411 {
412  unsigned int k = p.getRows();
413  if (rowNum != k) {
414  resize(k, false);
415  }
416 
417  memcpy(data, p.data, rowNum * sizeof(double));
418  return *this;
419 }
420 
422 {
423  *this = v;
424  return *this;
425 }
426 
428 {
429  for (unsigned int i = 0; i < rowNum; ++i) {
430  for (unsigned int j = 0; j < colNum; ++j) {
431  rowPtrs[i][j] = *x++;
432  }
433  }
434  return *this;
435 }
436 
438 {
439  resize(1, false);
440  data[0] = val;
441  return *this;
442 }
443 
445 {
446  resize(rowNum + 1, false);
447  data[rowNum - 1] = val;
448  return *this;
449 }
450 
452 {
453  double *d = data;
454 
455  for (unsigned int i = 0; i < rowNum; ++i) {
456  *(d++) = x;
457  }
458  return *this;
459 }
460 
461 std::vector<double> vpColVector::toStdVector() const
462 {
463  std::vector<double> v(this->size());
464 
465  unsigned int v_this_size = this->size();
466  for (unsigned int i = 0; i < v_this_size; ++i) {
467  v[i] = data[i];
468  }
469  return v;
470 }
471 
472 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
474 {
475  if (this != &other) {
476  free(data);
477  free(rowPtrs);
478 
479  rowNum = other.rowNum;
480  colNum = other.colNum;
481  rowPtrs = other.rowPtrs;
482  dsize = other.dsize;
483  data = other.data;
484 
485  other.rowNum = 0;
486  other.colNum = 0;
487  other.rowPtrs = nullptr;
488  other.dsize = 0;
489  other.data = nullptr;
490  }
491 
492  return *this;
493 }
494 
495 vpColVector &vpColVector::operator=(const std::initializer_list<double> &list)
496 {
497  resize(static_cast<unsigned int>(list.size()), false);
498  std::copy(list.begin(), list.end(), data);
499  return *this;
500 }
501 #endif
502 
504 {
505  if ((rowNum != v.rowNum) || (colNum != v.colNum) /* should not happen */) {
506  return false;
507  }
508 
509  for (unsigned int i = 0; i < rowNum; ++i) {
510  if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon())) {
511  return false;
512  }
513  }
514 
515  return true;
516 }
517 
518 bool vpColVector::operator==(double v) const
519 {
520  for (unsigned int i = 0; i < rowNum; ++i) {
521  if (!vpMath::equal(data[i], v, std::numeric_limits<double>::epsilon())) {
522  return false;
523  }
524  }
525 
526  return true;
527 }
528 
529 bool vpColVector::operator!=(const vpColVector &v) const { return !(*this == v); }
530 
531 bool vpColVector::operator!=(double v) const { return !(*this == v); }
532 
534 {
535  vpRowVector v(rowNum);
536  memcpy(v.data, data, rowNum * sizeof(double));
537  return v;
538 }
539 
540 vpRowVector vpColVector::transpose() const { return t(); }
541 
542 void vpColVector::transpose(vpRowVector &v) const { v = t(); }
543 
544 double vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
545 {
546  if (a.data == nullptr) {
547  throw(vpException(vpException::fatalError, "Cannot compute the dot product: first vector empty"));
548  }
549  if (b.data == nullptr) {
550  throw(vpException(vpException::fatalError, "Cannot compute the dot product: second vector empty"));
551  }
552  if (a.size() != b.size()) {
554  "Cannot compute the dot product between column vectors "
555  "with different dimensions (%d) and (%d)",
556  a.size(), b.size()));
557  }
558 
559  double *ad = a.data;
560  double *bd = b.data;
561 
562  double c = 0;
563  unsigned int a_rows_nbr = a.getRows();
564  for (unsigned int i = 0; i < a_rows_nbr; ++i) {
565  // Move the ad++ and bd++ increment/decrement into a dedicated expression-statement
566  c += (*ad) * (*bd);
567  ++ad;
568  ++bd;
569  }
570 
571  return c;
572 }
573 
574 
576 {
577  x /= sqrt(x.sumSquare());
578 
579  return x;
580 }
581 
583 {
584  double sum_square = sumSquare();
585 
586  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
587  *this /= sqrt(sum_square);
588  }
589 
590  // If sum = 0, we have a nul vector. So we return just.
591  return *this;
592 }
593 
595 {
596  if (v.data == nullptr) {
597  throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
598  }
599  vpColVector tab;
600  tab = v;
601  unsigned int nb_permutation = 1;
602  unsigned int i = 0;
603  while (nb_permutation != 0) {
604  nb_permutation = 0;
605  for (unsigned int j = v.getRows() - 1; j >= (i + 1); --j) {
606  if (tab[j] > tab[j - 1]) {
607  double tmp = tab[j];
608  tab[j] = tab[j - 1];
609  tab[j - 1] = tmp;
610  ++nb_permutation;
611  }
612  }
613  ++i;
614  }
615 
616  return tab;
617 }
618 
620 {
621  if (v.data == nullptr) {
622  throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
623  }
624  vpColVector tab;
625  tab = v;
626  unsigned int nb_permutation = 1;
627  unsigned int i = 0;
628  while (nb_permutation != 0) {
629  nb_permutation = 0;
630  for (unsigned int j = v.getRows() - 1; j >= (i + 1); --j) {
631  if (tab[j] < tab[j - 1]) {
632  double tmp = tab[j];
633  tab[j] = tab[j - 1];
634  tab[j - 1] = tmp;
635  ++nb_permutation;
636  }
637  }
638  ++i;
639  }
640 
641  return tab;
642 }
643 
644 void vpColVector::stack(double d)
645 {
646  this->resize(rowNum + 1, false);
647  (*this)[rowNum - 1] = d;
648 }
649 
650 void vpColVector::stack(const vpColVector &v) { *this = vpColVector::stack(*this, v); }
651 
653 {
654  vpColVector C;
655  vpColVector::stack(A, B, C);
656  return C;
657 }
658 
660 {
661  unsigned int nrA = A.getRows();
662  unsigned int nrB = B.getRows();
663 
664  if ((nrA == 0) && (nrB == 0)) {
665  C.resize(0);
666  return;
667  }
668 
669  if (nrB == 0) {
670  C = A;
671  return;
672  }
673 
674  if (nrA == 0) {
675  C = B;
676  return;
677  }
678 
679  // General case
680  C.resize(nrA + nrB, false);
681 
682  for (unsigned int i = 0; i < nrA; ++i) {
683  C[i] = A[i];
684  }
685 
686  for (unsigned int i = 0; i < nrB; ++i) {
687  C[nrA + i] = B[i];
688  }
689 }
690 
692 {
693  if ((v.data == nullptr) || (v.size() == 0)) {
694  throw(vpException(vpException::dimensionError, "Cannot compute column vector mean: vector empty"));
695  }
696 
697  // Use directly sum() function
698  double mean = v.sum();
699 
700  return mean / v.getRows();
701 }
702 
704 {
705  if ((v.data == nullptr) || (v.size() == 0)) {
706  throw(vpException(vpException::dimensionError, "Cannot compute column vector median: vector empty"));
707  }
708 
709  std::vector<double> vectorOfDoubles(v.data, v.data + v.rowNum);
710 
711  return vpMath::getMedian(vectorOfDoubles);
712 }
713 
714 double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection)
715 {
716  if ((v.data == nullptr) || (v.size() == 0)) {
717  throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty"));
718  }
719 #if defined(VISP_HAVE_SIMDLIB)
720  return SimdVectorStdev(v.data, v.rowNum, useBesselCorrection);
721 #else
722  double mean_value = v.sum() / v.size();
723  double sum_squared_diff = 0.0;
724  unsigned int v_size = v.size();
725  for (size_t i = 0; i < v_size; ++i) {
726  sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
727  }
728 
729  double divisor = static_cast<double>(v.size());
730  if (useBesselCorrection) {
731  divisor = divisor - 1;
732  }
733 
734  return std::sqrt(sum_squared_diff / divisor);
735 #endif
736 }
737 
739 {
740  vpMatrix M;
741  const unsigned int rows_size = 3;
742  if (v.getRows() != rows_size) {
743  throw(vpException(vpException::dimensionError, "Cannot compute skew vector of a non 3-dimension vector (%d)",
744  v.getRows()));
745  }
746 
747  M.resize(rows_size, rows_size, false, false);
748  const unsigned int index_0 = 0;
749  const unsigned int index_1 = 1;
750  const unsigned int index_2 = 2;
751  M[index_0][index_0] = 0;
752  M[index_0][index_1] = -v[index_2];
753  M[index_0][index_2] = v[index_1];
754  M[index_1][index_0] = v[index_2];
755  M[index_1][index_1] = 0;
756  M[index_1][index_2] = -v[index_0];
757  M[index_2][index_0] = -v[index_1];
758  M[index_2][index_1] = v[index_0];
759  M[index_2][index_2] = 0;
760 
761  return M;
762 }
763 
765 {
766  const unsigned int val_3 = 3;
767  if ((a.getRows() != val_3) || (b.getRows() != val_3)) {
769  "Cannot compute the cross product between column "
770  "vector with dimension %d and %d",
771  a.getRows(), b.getRows()));
772  }
773 
774  return vpColVector::skew(a) * b;
775 }
776 
777 vpMatrix vpColVector::reshape(unsigned int nrows, unsigned int ncols)
778 {
779  vpMatrix M(nrows, ncols);
780  reshape(M, nrows, ncols);
781  return M;
782 }
783 
784 void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
785 {
786  if (dsize != (nrows * ncols)) {
787  throw(vpException(vpException::dimensionError, "Cannot reshape (%dx1) column vector in (%dx%d) matrix", rowNum,
788  M.getRows(), M.getCols()));
789  }
790  if ((M.getRows() != nrows) || (M.getCols() != ncols)) {
791  M.resize(nrows, ncols, false, false);
792  }
793 
794  for (unsigned int j = 0; j < ncols; ++j) {
795  for (unsigned int i = 0; i < nrows; ++i) {
796  M[i][j] = data[(j * nrows) + i];
797  }
798  }
799 }
800 
801 void vpColVector::insert(unsigned int i, const vpColVector &v)
802 {
803  if ((i + v.size()) >(this->size())) {
804  throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
805  }
806 
807  if ((data != nullptr) && (v.data != nullptr) && (v.rowNum > 0)) {
808  memcpy(data + i, v.data, sizeof(double) * v.rowNum);
809  }
810 }
811 
812 int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) const
813 {
814  typedef std::string::size_type size_type;
815 
816  unsigned int m = getRows();
817  unsigned int n = 1;
818 
819  std::vector<std::string> values(m * n);
820  std::ostringstream oss;
821  std::ostringstream ossFixed;
822  std::ios_base::fmtflags original_flags = oss.flags();
823 
824  // ossFixed <<std::fixed
825  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
826 
827  size_type maxBefore = 0; // the length of the integral part
828  size_type maxAfter = 0; // number of decimals plus
829  // one place for the decimal point
830  for (unsigned int i = 0; i < m; ++i) {
831  oss.str("");
832  oss << (*this)[i];
833  if (oss.str().find("e") != std::string::npos) {
834  ossFixed.str("");
835  ossFixed << (*this)[i];
836  oss.str(ossFixed.str());
837  }
838 
839  values[i] = oss.str();
840  size_type thislen = values[i].size();
841  size_type p = values[i].find('.');
842 
843  if (p == std::string::npos) {
844  maxBefore = vpMath::maximum(maxBefore, thislen);
845  // maxAfter remains the same
846  }
847  else {
848  maxBefore = vpMath::maximum(maxBefore, p);
849  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
850  }
851  }
852 
853  size_type totalLength = length;
854  // increase totalLength according to maxBefore
855  totalLength = vpMath::maximum(totalLength, maxBefore);
856  // decrease maxAfter according to totalLength
857  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
858  if (maxAfter == 1) {
859  maxAfter = 0;
860  }
861 
862  // the following line is useful for debugging
863  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
864 
865  if (intro) {
866  s << intro;
867  }
868  s << "[" << m << "," << n << "]=\n";
869 
870  for (unsigned int i = 0; i < m; ++i) {
871  s << " ";
872  size_type p = values[i].find('.');
873  s.setf(std::ios::right, std::ios::adjustfield);
874  s.width(static_cast<std::streamsize>(maxBefore));
875  s << values[i].substr(0, p).c_str();
876 
877  if (maxAfter > 0) {
878  s.setf(std::ios::left, std::ios::adjustfield);
879  if (p != std::string::npos) {
880  s.width(static_cast<std::streamsize>(maxAfter));
881  s << values[i].substr(p, maxAfter).c_str();
882  }
883  else {
884  assert(maxAfter > 1);
885  s.width(static_cast<std::streamsize>(maxAfter));
886  s << ".0";
887  }
888  }
889 
890  s << ' ';
891 
892  s << std::endl;
893  }
894 
895  s.flags(original_flags); // restore s to standard state
896 
897  return static_cast<int>(maxBefore + maxAfter);
898 }
899 
900 double vpColVector::sum() const
901 {
902 #if defined(VISP_HAVE_SIMDLIB)
903  return SimdVectorSum(data, rowNum);
904 #else
905  double sum = 0.0;
906  for (unsigned int i = 0; i < rowNum; ++i) {
907  sum += (*this)[i];
908  }
909  return sum;
910 #endif
911 }
912 
914 {
915 #if defined(VISP_HAVE_SIMDLIB)
916  return SimdVectorSumSquare(data, rowNum);
917 #else
918  double sum_square = 0.0;
919  for (unsigned int i = 0; i < rowNum; ++i) {
920  sum_square += (*this)[i] * (*this)[i];
921  }
922  return sum_square;
923 #endif
924 }
925 
927 {
928  double norm = sumSquare();
929 
930  return sqrt(norm);
931 }
932 
934 {
935  if ((v.getRows() != rowNum) || (v.getCols() != colNum)) {
936  throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
937  }
938 
939  vpColVector out;
940  out.resize(rowNum, false);
941 #if defined(VISP_HAVE_SIMDLIB)
942  SimdVectorHadamard(data, v.data, rowNum, out.data);
943 #else
944  for (unsigned int i = 0; i < dsize; ++i) {
945  out.data[i] = data[i] * v.data[i];
946  }
947 #endif
948  return out;
949 }
950 
952 {
953  double norm = 0.0;
954  for (unsigned int i = 0; i < rowNum; ++i) {
955  double x = fabs((*this)[i]);
956  if (x > norm) {
957  norm = x;
958  }
959  }
960  return norm;
961 }
962 
963 std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
964 {
965  os << "vpColVector " << matrixName << " (" << this->getRows() << "); " << std::endl;
966 
967  unsigned int rows_nbr = this->getRows();
968  for (unsigned int i = 0; i < rows_nbr; ++i) {
969 
970  if (!octet) {
971  os << matrixName << "[" << i << "] = " << (*this)[i] << "; " << std::endl;
972  }
973  else {
974  for (unsigned int k = 0; k < sizeof(double); ++k) {
975  os << "((unsigned char*)&(" << matrixName << "[" << i << "]) )[" << k << "] = 0x" << std::hex
976  << static_cast<unsigned int>(((unsigned char *)&((*this)[i]))[k]) << "; " << std::endl;
977  }
978  }
979  }
980  std::cout << std::endl;
981  return os;
982 }
983 
984 std::ostream &vpColVector::csvPrint(std::ostream &os) const
985 {
986  unsigned int rows_nbr = this->getRows();
987  for (unsigned int i = 0; i < rows_nbr; ++i) {
988  os << (*this)[i];
989 
990  os << std::endl;
991  }
992  return os;
993 }
994 
995 std::ostream &vpColVector::maplePrint(std::ostream &os) const
996 {
997  unsigned int rows_nbr = this->getRows();
998  os << "([ " << std::endl;
999  for (unsigned int i = 0; i < rows_nbr; ++i) {
1000  os << "[";
1001  os << (*this)[i] << ", ";
1002  os << "]," << std::endl;
1003  }
1004  os << "])" << std::endl;
1005  return os;
1006 }
1007 
1008 std::ostream &vpColVector::matlabPrint(std::ostream &os) const
1009 {
1010  unsigned int rows_nbr = this->getRows();
1011  os << "[ ";
1012  for (unsigned int i = 0; i < rows_nbr; ++i) {
1013  os << (*this)[i] << ", ";
1014  if (this->getRows() != (i + 1)) {
1015  os << ";" << std::endl;
1016  }
1017  else {
1018  os << "]" << std::endl;
1019  }
1020  }
1021  return os;
1022 }
1023 
1024 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1025 
1026 void vpColVector::insert(const vpColVector &v, unsigned int i)
1027 {
1028  insert(i, v);
1029 }
1030 
1031 void vpColVector::insert(const vpColVector &v, unsigned int r, unsigned int c)
1032 {
1033  (void)c;
1034  insert(r, v);
1035 }
1036 
1037 double vpColVector::euclideanNorm() const { return frobeniusNorm(); }
1038 #endif // defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1039 
1040 vpColVector operator*(const double &x, const vpColVector &v)
1041 {
1042  vpColVector vout;
1043  vout = v * x;
1044  return vout;
1045 }
1046 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
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
unsigned int getRows() const
Definition: vpArray2D.h:347
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
double operator*(const vpColVector &v) const
static double dotProd(const vpColVector &a, const vpColVector &b)
vpColVector operator-() const
vpColVector & operator*=(double x)
std::ostream & matlabPrint(std::ostream &os) const
vpColVector & operator=(const vpColVector &v)
vpColVector operator/(double x) const
vpColVector & normalize()
static double median(const vpColVector &v)
vpColVector hadamard(const vpColVector &v) const
double sumSquare() const
std::ostream & csvPrint(std::ostream &os) const
std::ostream & maplePrint(std::ostream &os) const
vpColVector & operator,(double val)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
void init(const vpColVector &v, unsigned int r, unsigned int nrows)
bool operator==(const vpColVector &v) const
vpColVector & operator/=(double x)
static vpColVector invSort(const vpColVector &v)
void stack(double d)
bool operator!=(const vpColVector &v) const
static vpMatrix skew(const vpColVector &v)
std::vector< double > toStdVector() const
static double mean(const vpColVector &v)
vpColVector operator*(const double &x, const vpColVector &v)
vpColVector & operator+=(vpColVector v)
Definition: vpColVector.cpp:90
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double infinityNorm() const
vpColVector & operator<<(const vpColVector &v)
vpRowVector transpose() const
static double stdev(const vpColVector &v, bool useBesselCorrection=false)
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
double frobeniusNorm() const
vpColVector operator+(const vpColVector &v) const
Definition: vpColVector.cpp:60
vpColVector & operator-=(vpColVector v)
static vpColVector sort(const vpColVector &v)
void insert(unsigned int i, const vpColVector &v)
double sum() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:322
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Implementation of a generic rotation vector.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:124
Class that consider the case of a translation vector.