Visual Servoing Platform  version 3.2.1 under development (2019-10-21) under development (2019-10-21)
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 
150 
169 vpRowVector &vpRowVector::operator=(const std::initializer_list<double> &list)
170 {
171  resize(1, static_cast<unsigned int>(list.size()), false);
172  std::copy(list.begin(), list.end(), data);
173  return *this;
174 }
175 #endif
176 
177 bool vpRowVector::operator==(const vpRowVector &v) const {
178  if (colNum != v.colNum ||
179  rowNum != v.rowNum /* should not happen */)
180  return false;
181 
182  for (unsigned int i = 0; i < colNum; i++) {
183  if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon()))
184  return false;
185  }
186 
187  return true;
188 }
189 
190 bool vpRowVector::operator!=(const vpRowVector &v) const {
191  return !(*this == v);
192 }
193 
208 double vpRowVector::operator*(const vpColVector &x) const
209 {
210  unsigned int nelements = x.getRows();
211  if (getCols() != nelements) {
212  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum,
213  x.getRows()));
214  }
215 
216  double scalar = 0.0;
217 
218  for (unsigned int i = 0; i < nelements; i++) {
219  scalar += (*this)[i] * x[i];
220  }
221  return scalar;
222 }
239 {
240  vpRowVector c(M.getCols());
241 
242  if (colNum != M.getRows()) {
243  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum,
244  M.getRows(), M.getCols()));
245  }
246 
247  c = 0.0;
248 
249  for (unsigned int i = 0; i < colNum; i++) {
250  double bi = data[i]; // optimization em 5/12/2006
251  for (unsigned int j = 0; j < M.getCols(); j++) {
252  c[j] += bi * M[i][j];
253  }
254  }
255 
256  return c;
257 }
258 
279 {
280  vpRowVector v(colNum);
281 
282  double *vd = v.data;
283  double *d = data;
284 
285  for (unsigned int i = 0; i < colNum; i++)
286  *(vd++) = (*d++) * x;
287  return v;
288 }
289 
308 {
309  for (unsigned int i = 0; i < colNum; i++)
310  (*this)[i] *= x;
311  return (*this);
312 }
313 
334 {
335  vpRowVector v(colNum);
336 
337  double *vd = v.data;
338  double *d = data;
339 
340  for (unsigned int i = 0; i < colNum; i++)
341  *(vd++) = (*d++) / x;
342  return v;
343 }
344 
364 {
365  for (unsigned int i = 0; i < colNum; i++)
366  (*this)[i] /= x;
367  return (*this);
368 }
369 
381 {
382  vpRowVector A(colNum);
383 
384  double *vd = A.data;
385  double *d = data;
386 
387  for (unsigned int i = 0; i < colNum; i++)
388  *(vd++) = -(*d++);
389 
390  return A;
391 }
392 
398 {
399  if (getCols() != m.getCols()) {
400  throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(),
401  m.getCols()));
402  }
403 
404  vpRowVector v(colNum);
405 
406  for (unsigned int i = 0; i < colNum; i++)
407  v[i] = (*this)[i] - m[i];
408  return v;
409 }
410 
416 {
417  if (getCols() != v.getCols()) {
418  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
419  v.getCols()));
420  }
421 
422  vpRowVector r(colNum);
423 
424  for (unsigned int i = 0; i < colNum; i++)
425  r[i] = (*this)[i] + v[i];
426  return r;
427 }
428 
435 {
436  if (getCols() != v.getCols()) {
437  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
438  v.getCols()));
439  }
440 
441  for (unsigned int i = 0; i < colNum; i++)
442  (*this)[i] += v[i];
443  return (*this);
444 }
445 
452 {
453  if (getCols() != v.getCols()) {
454  throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(),
455  v.getCols()));
456  }
457 
458  for (unsigned int i = 0; i < colNum; i++)
459  (*this)[i] -= v[i];
460  return (*this);
461 }
462 
485 {
486  *this = v;
487  return *this;
488 }
489 
491 {
492  resize(1, false);
493  data[0] = val;
494  return *this;
495 }
496 
498 {
499  resize(colNum + 1, false);
500  data[colNum - 1] = val;
501  return *this;
502 }
503 
508 {
509  vpColVector v(colNum);
510  memcpy(v.data, data, colNum * sizeof(double));
511  return v;
512 }
513 
518 vpColVector vpRowVector::transpose() const { return t(); }
523 void vpRowVector::transpose(vpColVector &v) const { v = t(); }
524 
529 vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols())
530 {
531  for (unsigned int j = 0; j < M.getCols(); j++)
532  (*this)[j] = M[i][j];
533 }
541 {
542  if (M.getRows() != 1) {
543  throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
544  M.getCols(), M.getRows(), M.getCols()));
545  }
546 
547  for (unsigned int j = 0; j < M.getCols(); j++)
548  (*this)[j] = M[0][j];
549 }
550 
554 vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, (unsigned int)v.size())
555 {
556  for (unsigned int j = 0; j < v.size(); j++)
557  (*this)[j] = v[j];
558 }
562 vpRowVector::vpRowVector(const std::vector<float> &v) : vpArray2D<double>(1, (unsigned int)v.size())
563 {
564  for (unsigned int j = 0; j < v.size(); j++)
565  (*this)[j] = (double)(v[j]);
566 }
567 
581 vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
582 {
583  init(v, c, ncols);
584 }
585 
586 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
588 {
589  rowNum = v.rowNum;
590  colNum = v.colNum;
591  rowPtrs = v.rowPtrs;
592  dsize = v.dsize;
593  data = v.data;
594 
595  v.rowNum = 0;
596  v.colNum = 0;
597  v.rowPtrs = NULL;
598  v.dsize = 0;
599  v.data = NULL;
600 }
601 #endif
602 
613 {
614  x = x / sqrt(x.sumSquare());
615 
616  return x;
617 }
618 
628 {
629  double sum_square = sumSquare();
630  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
631  *this /= sqrt(sum_square);
632  }
633 
634  // If sum = 0, we have a nul vector. So we return just.
635  return *this;
636 }
637 
649 vpMatrix vpRowVector::reshape(unsigned int nrows, unsigned int ncols)
650 {
651  vpMatrix M(nrows, ncols);
652  reshape(M, nrows, ncols);
653  return M;
654 }
655 
699 void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
700 {
701  if (dsize != nrows * ncols) {
702  throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum,
703  M.getRows(), M.getCols()));
704  }
705  try {
706  if ((M.getRows() != nrows) || (M.getCols() != ncols))
707  M.resize(nrows, ncols);
708  } catch (...) {
709  throw;
710  }
711  for (unsigned int i = 0; i < nrows; i++)
712  for (unsigned int j = 0; j < ncols; j++)
713  M[i][j] = data[i * ncols + j];
714 }
715 
747 void vpRowVector::insert(unsigned int i, const vpRowVector &v)
748 {
749  if (i + v.size() > this->size())
751  "Unable to insert (1x%d) row vector in (1x%d) row "
752  "vector at position (%d)",
753  v.getCols(), colNum, i));
754  for (unsigned int j = 0; j < v.size(); j++)
755  (*this)[i + j] = v[j];
756 }
757 
762 std::vector<double> vpRowVector::toStdVector()
763 {
764  std::vector<double> v(this->size());
765 
766  for (unsigned int i = 0; i < this->size(); i++)
767  v[i] = data[i];
768  return v;
769 }
770 
787 void vpRowVector::stack(double d)
788 {
789  this->resize(colNum + 1, false);
790  (*this)[colNum - 1] = d;
791 }
792 
812 void vpRowVector::stack(const vpRowVector &v) { *this = vpRowVector::stack(*this, v); }
813 
835 {
836  vpRowVector C;
837  vpRowVector::stack(A, B, C);
838  return C;
839 }
840 
862 {
863  unsigned int nrA = A.getCols();
864  unsigned int nrB = B.getCols();
865 
866  if (nrA == 0 && nrB == 0) {
867  C.resize(0);
868  return;
869  }
870 
871  if (nrB == 0) {
872  C = A;
873  return;
874  }
875 
876  if (nrA == 0) {
877  C = B;
878  return;
879  }
880 
881  // General case
882  C.resize(nrA + nrB);
883 
884  for (unsigned int i = 0; i < nrA; i++)
885  C[i] = A[i];
886 
887  for (unsigned int i = 0; i < nrB; i++)
888  C[nrA + i] = B[i];
889 }
890 
895 {
896  if (v.data == NULL || v.size() == 0) {
897  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
898  }
899 
900  double mean = 0;
901  double *vd = v.data;
902  for (unsigned int i = 0; i < v.getCols(); i++)
903  mean += *(vd++);
904 
905  return mean / v.getCols();
906 }
907 
912 {
913  if (v.data == NULL || v.size() == 0) {
914  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
915  }
916 
917  std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);
918 
919  return vpMath::getMedian(vectorOfDoubles);
920 }
921 
925 double vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
926 {
927  if (v.data == NULL || v.size() == 0) {
928  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
929  }
930 
931  double mean_value = mean(v);
932  double sum_squared_diff = 0.0;
933  for (unsigned int i = 0; i < v.size(); i++) {
934  sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
935  }
936 
937  double divisor = (double)v.size();
938  if (useBesselCorrection && v.size() > 1) {
939  divisor = divisor - 1;
940  }
941 
942  return std::sqrt(sum_squared_diff / divisor);
943 }
944 
964 int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) const
965 {
966  typedef std::string::size_type size_type;
967 
968  unsigned int m = 1;
969  unsigned int n = getCols();
970 
971  std::vector<std::string> values(m * n);
972  std::ostringstream oss;
973  std::ostringstream ossFixed;
974  std::ios_base::fmtflags original_flags = oss.flags();
975 
976  // ossFixed <<std::fixed;
977  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
978 
979  size_type maxBefore = 0; // the length of the integral part
980  size_type maxAfter = 0; // number of decimals plus
981  // one place for the decimal point
982  for (unsigned int j = 0; j < n; ++j) {
983  oss.str("");
984  oss << (*this)[j];
985  if (oss.str().find("e") != std::string::npos) {
986  ossFixed.str("");
987  ossFixed << (*this)[j];
988  oss.str(ossFixed.str());
989  }
990 
991  values[j] = oss.str();
992  size_type thislen = values[j].size();
993  size_type p = values[j].find('.');
994 
995  if (p == std::string::npos) {
996  maxBefore = vpMath::maximum(maxBefore, thislen);
997  // maxAfter remains the same
998  } else {
999  maxBefore = vpMath::maximum(maxBefore, p);
1000  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
1001  }
1002  }
1003 
1004  size_type totalLength = length;
1005  // increase totalLength according to maxBefore
1006  totalLength = vpMath::maximum(totalLength, maxBefore);
1007  // decrease maxAfter according to totalLength
1008  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
1009  if (maxAfter == 1)
1010  maxAfter = 0;
1011 
1012  // the following line is useful for debugging
1013  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
1014 
1015  if (intro)
1016  s << intro;
1017  s << "[" << m << "," << n << "]=\n";
1018 
1019  s << " ";
1020  for (unsigned int j = 0; j < n; j++) {
1021  size_type p = values[j].find('.');
1022  s.setf(std::ios::right, std::ios::adjustfield);
1023  s.width((std::streamsize)maxBefore);
1024  s << values[j].substr(0, p).c_str();
1025 
1026  if (maxAfter > 0) {
1027  s.setf(std::ios::left, std::ios::adjustfield);
1028  if (p != std::string::npos) {
1029  s.width((std::streamsize)maxAfter);
1030  s << values[j].substr(p, maxAfter).c_str();
1031  } else {
1032  assert(maxAfter > 1);
1033  s.width((std::streamsize)maxAfter);
1034  s << ".0";
1035  }
1036  }
1037 
1038  s << ' ';
1039  }
1040  s << std::endl;
1041 
1042  s.flags(original_flags); // restore s to standard state
1043 
1044  return (int)(maxBefore + maxAfter);
1045 }
1046 
1050 vpRowVector operator*(const double &x, const vpRowVector &v)
1051 {
1052  vpRowVector vout;
1053  vout = v * x;
1054  return vout;
1055 }
1056 
1062 double vpRowVector::sum() const
1063 {
1064  double sum = 0.0;
1065 
1066  for (unsigned int j = 0; j < colNum; j++) {
1067  sum += rowPtrs[0][j];
1068  }
1069 
1070  return sum;
1071 }
1072 
1080 {
1081  double sum_square = 0.0;
1082 
1083  for (unsigned int j = 0; j < colNum; j++) {
1084  double x = rowPtrs[0][j];
1085  sum_square += x * x;
1086  }
1087 
1088  return sum_square;
1089 }
1090 
1101 {
1102  return frobeniusNorm();
1103 }
1104 
1111 {
1112  double norm = sumSquare();
1113 
1114  return sqrt(norm);
1115 }
1116 
1152 void vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
1153 {
1154  unsigned int cncols = c + ncols;
1155 
1156  if (cncols > v.getCols())
1157  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector",
1158  cncols, v.getCols()));
1159  resize(ncols);
1160  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
1161  return; // Noting to do
1162  for (unsigned int i = 0; i < ncols; i++)
1163  (*this)[i] = v[i + c];
1164 }
1165 
1196 std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
1197 {
1198  os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl;
1199 
1200  for (unsigned int j = 0; j < this->getCols(); ++j) {
1201  if (!octet) {
1202  os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl;
1203  } else {
1204  for (unsigned int k = 0; k < sizeof(double); ++k) {
1205  os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex
1206  << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl;
1207  }
1208  }
1209  }
1210  std::cout << std::endl;
1211  return os;
1212 }
1213 
1238 std::ostream &vpRowVector::csvPrint(std::ostream &os) const
1239 {
1240  for (unsigned int j = 0; j < this->getCols(); ++j) {
1241  os << (*this)[j];
1242  if (!(j == (this->getCols() - 1)))
1243  os << ", ";
1244  }
1245  os << std::endl;
1246  return os;
1247 }
1248 
1272 std::ostream &vpRowVector::maplePrint(std::ostream &os) const
1273 {
1274  os << "([ " << std::endl;
1275  os << "[";
1276  for (unsigned int j = 0; j < this->getCols(); ++j) {
1277  os << (*this)[j] << ", ";
1278  }
1279  os << "]," << std::endl;
1280  os << "])" << std::endl;
1281  return os;
1282 }
1283 
1314 std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
1315 {
1316  os << "[ ";
1317  for (unsigned int j = 0; j < this->getCols(); ++j) {
1318  os << (*this)[j] << ", ";
1319  }
1320  os << "]" << std::endl;
1321  return os;
1322 }
vpRowVector & operator/=(double x)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRowVector & normalize()
static double median(const vpRowVector &v)
vpRowVector operator+(const vpRowVector &v) const
double frobeniusNorm() const
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
std::vector< double > toStdVector()
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
static double mean(const vpRowVector &v)
vp_deprecated void init()
Definition: vpRowVector.h:321
static double stdev(const vpRowVector &v, const bool useBesselCorrection=false)
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
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
vpRowVector operator/(const double x) const
std::ostream & matlabPrint(std::ostream &os) const
int print(std::ostream &s, unsigned int length, char const *intro=0) const
std::ostream & csvPrint(std::ostream &os) const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
unsigned int getCols() const
Definition: vpArray2D.h:279
std::ostream & maplePrint(std::ostream &os) const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
bool operator!=(const vpRowVector &v) const
double operator*(const vpColVector &x) const
bool operator==(const vpRowVector &v) const
Comparison operator.
vpRowVector & operator-=(vpRowVector v)
vpColVector transpose() const
void stack(double d)
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpRowVector.h:271
vpRowVector & operator+=(vpRowVector v)
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
vpColVector t() const
vpRowVector & operator<<(const vpRowVector &v)
double sum() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpRowVector & operator*=(double x)
vpRowVector & operator=(const vpRowVector &v)
Copy operator. Allow operation such as A = v.
Definition: vpRowVector.cpp:58
double sumSquare() const
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
vp_deprecated double euclideanNorm() const
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
vpRowVector & operator,(double val)
vpRowVector()
Basic constructor that creates an empty 0-size row vector.
Definition: vpRowVector.h:119
void insert(unsigned int i, const vpRowVector &v)
vpRowVector operator-() const