Visual Servoing Platform  version 3.5.1 under development (2022-12-06)
vpRowVector.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  * Operation on row vectors.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 #include <assert.h>
45 #include <cmath>
46 #include <sstream>
47 #include <stdlib.h>
48 #include <string.h>
49 
50 #include <visp3/core/vpArray2D.h>
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpException.h>
54 #include <visp3/core/vpMatrix.h>
55 #include <visp3/core/vpRowVector.h>
56 
59 {
60  unsigned int k = v.colNum;
61  if (colNum != k) {
62  try {
63  resize(k);
64  } catch (...) {
65  throw;
66  }
67  }
68 
69  memcpy(data, v.data, colNum * sizeof(double));
70 
71  return *this;
72 }
73 
82 {
83  if (M.getRows() != 1) {
84  throw(vpException(vpException::dimensionError, "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
85  M.getCols(), M.getRows(), M.getCols()));
86  }
87 
88  if (M.getCols() != colNum)
89  resize(M.getCols());
90 
91  memcpy(data, M.data, colNum * sizeof(double));
92  return *this;
93 }
94 
98 vpRowVector &vpRowVector::operator=(const std::vector<double> &v)
99 {
100  resize((unsigned int)v.size());
101  for (unsigned int i = 0; i < v.size(); i++)
102  (*this)[i] = v[i];
103  return *this;
104 }
108 vpRowVector &vpRowVector::operator=(const std::vector<float> &v)
109 {
110  resize((unsigned int)v.size());
111  for (unsigned int i = 0; i < v.size(); i++)
112  (*this)[i] = (float)v[i];
113  return *this;
114 }
115 
118 {
119  for (unsigned int i = 0; i < rowNum; i++) {
120  for (unsigned int j = 0; j < colNum; j++) {
121  rowPtrs[i][j] = x;
122  }
123  }
124  return *this;
125 }
126 
127 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
129 {
130  if (this != &other) {
131  free(data);
132  free(rowPtrs);
133 
134  rowNum = other.rowNum;
135  colNum = other.colNum;
136  rowPtrs = other.rowPtrs;
137  dsize = other.dsize;
138  data = other.data;
139 
140  other.rowNum = 0;
141  other.colNum = 0;
142  other.rowPtrs = NULL;
143  other.dsize = 0;
144  other.data = NULL;
145  }
146 
147  return *this;
148 }
149 
168 vpRowVector &vpRowVector::operator=(const std::initializer_list<double> &list)
169 {
170  resize(1, static_cast<unsigned int>(list.size()), false);
171  std::copy(list.begin(), list.end(), data);
172  return *this;
173 }
174 #endif
175 
177 {
178  if (colNum != v.colNum || rowNum != v.rowNum /* should not happen */)
179  return false;
180 
181  for (unsigned int i = 0; i < colNum; i++) {
182  if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon()))
183  return false;
184  }
185 
186  return true;
187 }
188 
189 bool vpRowVector::operator!=(const vpRowVector &v) const { return !(*this == v); }
190 
205 double vpRowVector::operator*(const vpColVector &x) const
206 {
207  unsigned int nelements = x.getRows();
208  if (getCols() != nelements) {
209  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum,
210  x.getRows()));
211  }
212 
213  double scalar = 0.0;
214 
215  for (unsigned int i = 0; i < nelements; i++) {
216  scalar += (*this)[i] * x[i];
217  }
218  return scalar;
219 }
236 {
237  vpRowVector c(M.getCols());
238 
239  if (colNum != M.getRows()) {
240  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum,
241  M.getRows(), M.getCols()));
242  }
243 
244  c = 0.0;
245 
246  for (unsigned int i = 0; i < colNum; i++) {
247  double bi = data[i]; // optimization em 5/12/2006
248  for (unsigned int j = 0; j < M.getCols(); j++) {
249  c[j] += bi * M[i][j];
250  }
251  }
252 
253  return c;
254 }
255 
276 {
277  vpRowVector v(colNum);
278 
279  double *vd = v.data;
280  double *d = data;
281 
282  for (unsigned int i = 0; i < colNum; i++)
283  *(vd++) = (*d++) * x;
284  return v;
285 }
286 
305 {
306  for (unsigned int i = 0; i < colNum; i++)
307  (*this)[i] *= x;
308  return (*this);
309 }
310 
331 {
332  vpRowVector v(colNum);
333 
334  double *vd = v.data;
335  double *d = data;
336 
337  for (unsigned int i = 0; i < colNum; i++)
338  *(vd++) = (*d++) / x;
339  return v;
340 }
341 
361 {
362  for (unsigned int i = 0; i < colNum; i++)
363  (*this)[i] /= x;
364  return (*this);
365 }
366 
378 {
379  vpRowVector A(colNum);
380 
381  double *vd = A.data;
382  double *d = data;
383 
384  for (unsigned int i = 0; i < colNum; i++)
385  *(vd++) = -(*d++);
386 
387  return A;
388 }
389 
395 {
396  if (getCols() != m.getCols()) {
397  throw(vpException(vpException::dimensionError, "Cannot subtract (1x%d) row vector to (1x%d) row vector", getCols(),
398  m.getCols()));
399  }
400 
401  vpRowVector v(colNum);
402 
403  for (unsigned int i = 0; i < colNum; i++)
404  v[i] = (*this)[i] - m[i];
405  return v;
406 }
407 
413 {
414  if (getCols() != v.getCols()) {
415  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
416  v.getCols()));
417  }
418 
419  vpRowVector r(colNum);
420 
421  for (unsigned int i = 0; i < colNum; i++)
422  r[i] = (*this)[i] + v[i];
423  return r;
424 }
425 
432 {
433  if (getCols() != v.getCols()) {
434  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
435  v.getCols()));
436  }
437 
438  for (unsigned int i = 0; i < colNum; i++)
439  (*this)[i] += v[i];
440  return (*this);
441 }
442 
449 {
450  if (getCols() != v.getCols()) {
451  throw(vpException(vpException::dimensionError, "Cannot subtract (1x%d) row vector to (1x%d) row vector", getCols(),
452  v.getCols()));
453  }
454 
455  for (unsigned int i = 0; i < colNum; i++)
456  (*this)[i] -= v[i];
457  return (*this);
458 }
459 
482 {
483  *this = v;
484  return *this;
485 }
486 
488 {
489  resize(1, false);
490  data[0] = val;
491  return *this;
492 }
493 
495 {
496  resize(colNum + 1, false);
497  data[colNum - 1] = val;
498  return *this;
499 }
500 
505 {
506  vpColVector v(colNum);
507  memcpy(v.data, data, colNum * sizeof(double));
508  return v;
509 }
510 
515 vpColVector vpRowVector::transpose() const { return t(); }
520 void vpRowVector::transpose(vpColVector &v) const { v = t(); }
521 
526 vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols())
527 {
528  for (unsigned int j = 0; j < M.getCols(); j++)
529  (*this)[j] = M[i][j];
530 }
537 vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D<double>(1, M.getCols())
538 {
539  if (M.getRows() != 1) {
540  throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
541  M.getCols(), M.getRows(), M.getCols()));
542  }
543 
544  for (unsigned int j = 0; j < M.getCols(); j++)
545  (*this)[j] = M[0][j];
546 }
547 
551 vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, (unsigned int)v.size())
552 {
553  for (unsigned int j = 0; j < v.size(); j++)
554  (*this)[j] = v[j];
555 }
559 vpRowVector::vpRowVector(const std::vector<float> &v) : vpArray2D<double>(1, (unsigned int)v.size())
560 {
561  for (unsigned int j = 0; j < v.size(); j++)
562  (*this)[j] = (double)(v[j]);
563 }
564 
578 vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
579 {
580  init(v, c, ncols);
581 }
582 
583 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
585 {
586  rowNum = v.rowNum;
587  colNum = v.colNum;
588  rowPtrs = v.rowPtrs;
589  dsize = v.dsize;
590  data = v.data;
591 
592  v.rowNum = 0;
593  v.colNum = 0;
594  v.rowPtrs = NULL;
595  v.dsize = 0;
596  v.data = NULL;
597 }
598 #endif
599 
610 {
611  x = x / sqrt(x.sumSquare());
612 
613  return x;
614 }
615 
625 {
626  double sum_square = sumSquare();
627  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
628  *this /= sqrt(sum_square);
629  }
630 
631  // If sum = 0, we have a nul vector. So we return just.
632  return *this;
633 }
634 
646 vpMatrix vpRowVector::reshape(unsigned int nrows, unsigned int ncols)
647 {
648  vpMatrix M(nrows, ncols);
649  reshape(M, nrows, ncols);
650  return M;
651 }
652 
696 void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
697 {
698  if (dsize != nrows * ncols) {
699  throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum,
700  M.getRows(), M.getCols()));
701  }
702  try {
703  if ((M.getRows() != nrows) || (M.getCols() != ncols))
704  M.resize(nrows, ncols);
705  } catch (...) {
706  throw;
707  }
708  for (unsigned int i = 0; i < nrows; i++)
709  for (unsigned int j = 0; j < ncols; j++)
710  M[i][j] = data[i * ncols + j];
711 }
712 
744 void vpRowVector::insert(unsigned int i, const vpRowVector &v)
745 {
746  if (i + v.size() > this->size())
748  "Unable to insert (1x%d) row vector in (1x%d) row "
749  "vector at position (%d)",
750  v.getCols(), colNum, i));
751  for (unsigned int j = 0; j < v.size(); j++)
752  (*this)[i + j] = v[j];
753 }
754 
759 std::vector<double> vpRowVector::toStdVector() const
760 {
761  std::vector<double> v(this->size());
762 
763  for (unsigned int i = 0; i < this->size(); i++)
764  v[i] = data[i];
765  return v;
766 }
767 
784 void vpRowVector::stack(double d)
785 {
786  this->resize(colNum + 1, false);
787  (*this)[colNum - 1] = d;
788 }
789 
809 void vpRowVector::stack(const vpRowVector &v) { *this = vpRowVector::stack(*this, v); }
810 
832 {
833  vpRowVector C;
834  vpRowVector::stack(A, B, C);
835  return C;
836 }
837 
859 {
860  unsigned int nrA = A.getCols();
861  unsigned int nrB = B.getCols();
862 
863  if (nrA == 0 && nrB == 0) {
864  C.resize(0);
865  return;
866  }
867 
868  if (nrB == 0) {
869  C = A;
870  return;
871  }
872 
873  if (nrA == 0) {
874  C = B;
875  return;
876  }
877 
878  // General case
879  C.resize(nrA + nrB);
880 
881  for (unsigned int i = 0; i < nrA; i++)
882  C[i] = A[i];
883 
884  for (unsigned int i = 0; i < nrB; i++)
885  C[nrA + i] = B[i];
886 }
887 
892 {
893  if (v.data == NULL || v.size() == 0) {
894  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
895  }
896 
897  double mean = 0;
898  double *vd = v.data;
899  for (unsigned int i = 0; i < v.getCols(); i++)
900  mean += *(vd++);
901 
902  return mean / v.getCols();
903 }
904 
909 {
910  if (v.data == NULL || v.size() == 0) {
911  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
912  }
913 
914  std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);
915 
916  return vpMath::getMedian(vectorOfDoubles);
917 }
918 
922 double vpRowVector::stdev(const vpRowVector &v, bool useBesselCorrection)
923 {
924  if (v.data == NULL || v.size() == 0) {
925  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
926  }
927 
928  double mean_value = mean(v);
929  double sum_squared_diff = 0.0;
930  for (unsigned int i = 0; i < v.size(); i++) {
931  sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
932  }
933 
934  double divisor = (double)v.size();
935  if (useBesselCorrection && v.size() > 1) {
936  divisor = divisor - 1;
937  }
938 
939  return std::sqrt(sum_squared_diff / divisor);
940 }
941 
961 int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) const
962 {
963  typedef std::string::size_type size_type;
964 
965  unsigned int m = 1;
966  unsigned int n = getCols();
967 
968  std::vector<std::string> values(m * n);
969  std::ostringstream oss;
970  std::ostringstream ossFixed;
971  std::ios_base::fmtflags original_flags = oss.flags();
972 
973  // ossFixed <<std::fixed;
974  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
975 
976  size_type maxBefore = 0; // the length of the integral part
977  size_type maxAfter = 0; // number of decimals plus
978  // one place for the decimal point
979  for (unsigned int j = 0; j < n; ++j) {
980  oss.str("");
981  oss << (*this)[j];
982  if (oss.str().find("e") != std::string::npos) {
983  ossFixed.str("");
984  ossFixed << (*this)[j];
985  oss.str(ossFixed.str());
986  }
987 
988  values[j] = oss.str();
989  size_type thislen = values[j].size();
990  size_type p = values[j].find('.');
991 
992  if (p == std::string::npos) {
993  maxBefore = vpMath::maximum(maxBefore, thislen);
994  // maxAfter remains the same
995  } else {
996  maxBefore = vpMath::maximum(maxBefore, p);
997  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
998  }
999  }
1000 
1001  size_type totalLength = length;
1002  // increase totalLength according to maxBefore
1003  totalLength = vpMath::maximum(totalLength, maxBefore);
1004  // decrease maxAfter according to totalLength
1005  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
1006  if (maxAfter == 1)
1007  maxAfter = 0;
1008 
1009  // the following line is useful for debugging
1010  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
1011 
1012  if (intro)
1013  s << intro;
1014  s << "[" << m << "," << n << "]=\n";
1015 
1016  s << " ";
1017  for (unsigned int j = 0; j < n; j++) {
1018  size_type p = values[j].find('.');
1019  s.setf(std::ios::right, std::ios::adjustfield);
1020  s.width((std::streamsize)maxBefore);
1021  s << values[j].substr(0, p).c_str();
1022 
1023  if (maxAfter > 0) {
1024  s.setf(std::ios::left, std::ios::adjustfield);
1025  if (p != std::string::npos) {
1026  s.width((std::streamsize)maxAfter);
1027  s << values[j].substr(p, maxAfter).c_str();
1028  } else {
1029  assert(maxAfter > 1);
1030  s.width((std::streamsize)maxAfter);
1031  s << ".0";
1032  }
1033  }
1034 
1035  s << ' ';
1036  }
1037  s << std::endl;
1038 
1039  s.flags(original_flags); // restore s to standard state
1040 
1041  return (int)(maxBefore + maxAfter);
1042 }
1043 
1047 vpRowVector operator*(const double &x, const vpRowVector &v)
1048 {
1049  vpRowVector vout;
1050  vout = v * x;
1051  return vout;
1052 }
1053 
1059 double vpRowVector::sum() const
1060 {
1061  double sum = 0.0;
1062 
1063  for (unsigned int j = 0; j < colNum; j++) {
1064  sum += rowPtrs[0][j];
1065  }
1066 
1067  return sum;
1068 }
1069 
1077 {
1078  double sum_square = 0.0;
1079 
1080  for (unsigned int j = 0; j < colNum; j++) {
1081  double x = rowPtrs[0][j];
1082  sum_square += x * x;
1083  }
1084 
1085  return sum_square;
1086 }
1087 
1097 double vpRowVector::euclideanNorm() const { return frobeniusNorm(); }
1098 
1105 {
1106  double norm = sumSquare();
1107 
1108  return sqrt(norm);
1109 }
1110 
1146 void vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
1147 {
1148  unsigned int cncols = c + ncols;
1149 
1150  if (cncols > v.getCols())
1151  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector",
1152  cncols, v.getCols()));
1153  resize(ncols);
1154  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
1155  return; // Noting to do
1156  for (unsigned int i = 0; i < ncols; i++)
1157  (*this)[i] = v[i + c];
1158 }
1159 
1190 std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
1191 {
1192  os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl;
1193 
1194  for (unsigned int j = 0; j < this->getCols(); ++j) {
1195  if (!octet) {
1196  os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl;
1197  } else {
1198  for (unsigned int k = 0; k < sizeof(double); ++k) {
1199  os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex
1200  << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl;
1201  }
1202  }
1203  }
1204  std::cout << std::endl;
1205  return os;
1206 }
1207 
1232 std::ostream &vpRowVector::csvPrint(std::ostream &os) const
1233 {
1234  for (unsigned int j = 0; j < this->getCols(); ++j) {
1235  os << (*this)[j];
1236  if (!(j == (this->getCols() - 1)))
1237  os << ", ";
1238  }
1239  os << std::endl;
1240  return os;
1241 }
1242 
1266 std::ostream &vpRowVector::maplePrint(std::ostream &os) const
1267 {
1268  os << "([ " << std::endl;
1269  os << "[";
1270  for (unsigned int j = 0; j < this->getCols(); ++j) {
1271  os << (*this)[j] << ", ";
1272  }
1273  os << "]," << std::endl;
1274  os << "])" << std::endl;
1275  return os;
1276 }
1277 
1308 std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
1309 {
1310  os << "[ ";
1311  for (unsigned int j = 0; j < this->getCols(); ++j) {
1312  os << (*this)[j] << ", ";
1313  }
1314  os << "]" << std::endl;
1315  return os;
1316 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:281
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
unsigned int getRows() const
Definition: vpArray2D.h:291
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, double scale)
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:262
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:171
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
bool operator==(const vpRowVector &v) const
Comparison operator.
vpRowVector & operator/=(double x)
double frobeniusNorm() const
vpColVector t() const
vp_deprecated double euclideanNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
vpRowVector & operator+=(vpRowVector v)
bool operator!=(const vpRowVector &v) const
vpRowVector operator-() const
static double mean(const vpRowVector &v)
void insert(unsigned int i, const vpRowVector &v)
vpRowVector & operator,(double val)
vpRowVector()
Basic constructor that creates an empty 0-size row vector.
Definition: vpRowVector.h:119
vpColVector transpose() const
void stack(double d)
vpRowVector operator+(const vpRowVector &v) const
vpRowVector & operator=(const vpRowVector &v)
Copy operator. Allow operation such as A = v.
Definition: vpRowVector.cpp:58
vp_deprecated void init()
Definition: vpRowVector.h:318
std::ostream & maplePrint(std::ostream &os) const
double sum() const
double operator*(const vpColVector &x) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
vpRowVector & operator*=(double x)
double sumSquare() const
vpRowVector & normalize()
std::ostream & csvPrint(std::ostream &os) const
vpRowVector operator/(double x) const
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
static double median(const vpRowVector &v)
std::vector< double > toStdVector() const
vpRowVector & operator<<(const vpRowVector &v)
static double stdev(const vpRowVector &v, bool useBesselCorrection=false)
vpRowVector & operator-=(vpRowVector v)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
std::ostream & matlabPrint(std::ostream &os) const