Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
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 
191 double vpRowVector::operator*(const vpColVector &x) const
192 {
193  unsigned int nelements = x.getRows();
194  if (getCols() != nelements) {
195  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum,
196  x.getRows()));
197  }
198 
199  double scalar = 0.0;
200 
201  for (unsigned int i = 0; i < nelements; i++) {
202  scalar += (*this)[i] * x[i];
203  }
204  return scalar;
205 }
222 {
223  vpRowVector c(M.getCols());
224 
225  if (colNum != M.getRows()) {
226  throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum,
227  M.getRows(), M.getCols()));
228  }
229 
230  c = 0.0;
231 
232  for (unsigned int i = 0; i < colNum; i++) {
233  double bi = data[i]; // optimization em 5/12/2006
234  for (unsigned int j = 0; j < M.getCols(); j++) {
235  c[j] += bi * M[i][j];
236  }
237  }
238 
239  return c;
240 }
241 
262 {
263  vpRowVector v(colNum);
264 
265  double *vd = v.data;
266  double *d = data;
267 
268  for (unsigned int i = 0; i < colNum; i++)
269  *(vd++) = (*d++) * x;
270  return v;
271 }
272 
291 {
292  for (unsigned int i = 0; i < colNum; i++)
293  (*this)[i] *= x;
294  return (*this);
295 }
296 
317 {
318  vpRowVector v(colNum);
319 
320  double *vd = v.data;
321  double *d = data;
322 
323  for (unsigned int i = 0; i < colNum; i++)
324  *(vd++) = (*d++) / x;
325  return v;
326 }
327 
347 {
348  for (unsigned int i = 0; i < colNum; i++)
349  (*this)[i] /= x;
350  return (*this);
351 }
352 
364 {
365  vpRowVector A(colNum);
366 
367  double *vd = A.data;
368  double *d = data;
369 
370  for (unsigned int i = 0; i < colNum; i++)
371  *(vd++) = -(*d++);
372 
373  return A;
374 }
375 
381 {
382  if (getCols() != m.getCols()) {
383  throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(),
384  m.getCols()));
385  }
386 
387  vpRowVector v(colNum);
388 
389  for (unsigned int i = 0; i < colNum; i++)
390  v[i] = (*this)[i] - m[i];
391  return v;
392 }
393 
399 {
400  if (getCols() != v.getCols()) {
401  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
402  v.getCols()));
403  }
404 
405  vpRowVector r(colNum);
406 
407  for (unsigned int i = 0; i < colNum; i++)
408  r[i] = (*this)[i] + v[i];
409  return r;
410 }
411 
418 {
419  if (getCols() != v.getCols()) {
420  throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
421  v.getCols()));
422  }
423 
424  for (unsigned int i = 0; i < colNum; i++)
425  (*this)[i] += v[i];
426  return (*this);
427 }
428 
435 {
436  if (getCols() != v.getCols()) {
437  throw(vpException(vpException::dimensionError, "Cannot substract (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 
468 {
469  *this = v;
470  return *this;
471 }
472 
474 {
475  resize(1, false);
476  data[0] = val;
477  return *this;
478 }
479 
481 {
482  resize(colNum + 1, false);
483  data[colNum - 1] = val;
484  return *this;
485 }
486 
491 {
492  vpColVector v(colNum);
493  memcpy(v.data, data, colNum * sizeof(double));
494  return v;
495 }
496 
501 vpColVector vpRowVector::transpose() const { return t(); }
506 void vpRowVector::transpose(vpColVector &v) const { v = t(); }
507 
512 vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols())
513 {
514  for (unsigned int j = 0; j < M.getCols(); j++)
515  (*this)[j] = M[i][j];
516 }
524 {
525  if (M.getRows() != 1) {
526  throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
527  M.getCols(), M.getRows(), M.getCols()));
528  }
529 
530  for (unsigned int j = 0; j < M.getCols(); j++)
531  (*this)[j] = M[0][j];
532 }
533 
537 vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, (unsigned int)v.size())
538 {
539  for (unsigned int j = 0; j < v.size(); j++)
540  (*this)[j] = v[j];
541 }
545 vpRowVector::vpRowVector(const std::vector<float> &v) : vpArray2D<double>(1, (unsigned int)v.size())
546 {
547  for (unsigned int j = 0; j < v.size(); j++)
548  (*this)[j] = (double)(v[j]);
549 }
550 
564 vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
565 {
566  init(v, c, ncols);
567 }
568 
569 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
571 {
572  rowNum = v.rowNum;
573  colNum = v.colNum;
574  rowPtrs = v.rowPtrs;
575  dsize = v.dsize;
576  data = v.data;
577 
578  v.rowNum = 0;
579  v.colNum = 0;
580  v.rowPtrs = NULL;
581  v.dsize = 0;
582  v.data = NULL;
583 }
584 #endif
585 
596 {
597  x = x / sqrt(x.sumSquare());
598 
599  return x;
600 }
601 
611 {
612  double sum_square = sumSquare();
613  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
614  *this /= sqrt(sum_square);
615  }
616 
617  // If sum = 0, we have a nul vector. So we return just.
618  return *this;
619 }
620 
632 vpMatrix vpRowVector::reshape(unsigned int nrows, unsigned int ncols)
633 {
634  vpMatrix M(nrows, ncols);
635  reshape(M, nrows, ncols);
636  return M;
637 }
638 
682 void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
683 {
684  if (dsize != nrows * ncols) {
685  throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum,
686  M.getRows(), M.getCols()));
687  }
688  try {
689  if ((M.getRows() != nrows) || (M.getCols() != ncols))
690  M.resize(nrows, ncols);
691  } catch (...) {
692  throw;
693  }
694  for (unsigned int i = 0; i < nrows; i++)
695  for (unsigned int j = 0; j < ncols; j++)
696  M[i][j] = data[i * ncols + j];
697 }
698 
730 void vpRowVector::insert(unsigned int i, const vpRowVector &v)
731 {
732  if (i + v.size() > this->size())
734  "Unable to insert (1x%d) row vector in (1x%d) row "
735  "vector at position (%d)",
736  v.getCols(), colNum, i));
737  for (unsigned int j = 0; j < v.size(); j++)
738  (*this)[i + j] = v[j];
739 }
740 
745 std::vector<double> vpRowVector::toStdVector()
746 {
747  std::vector<double> v(this->size());
748 
749  for (unsigned int i = 0; i < this->size(); i++)
750  v[i] = data[i];
751  return v;
752 }
753 
770 void vpRowVector::stack(double d)
771 {
772  this->resize(colNum + 1, false);
773  (*this)[colNum - 1] = d;
774 }
775 
795 void vpRowVector::stack(const vpRowVector &v) { *this = vpRowVector::stack(*this, v); }
796 
818 {
819  vpRowVector C;
820  vpRowVector::stack(A, B, C);
821  return C;
822 }
823 
845 {
846  unsigned int nrA = A.getCols();
847  unsigned int nrB = B.getCols();
848 
849  if (nrA == 0 && nrB == 0) {
850  C.resize(0);
851  return;
852  }
853 
854  if (nrB == 0) {
855  C = A;
856  return;
857  }
858 
859  if (nrA == 0) {
860  C = B;
861  return;
862  }
863 
864  // General case
865  C.resize(nrA + nrB);
866 
867  for (unsigned int i = 0; i < nrA; i++)
868  C[i] = A[i];
869 
870  for (unsigned int i = 0; i < nrB; i++)
871  C[nrA + i] = B[i];
872 }
873 
878 {
879  if (v.data == NULL || v.size() == 0) {
880  throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
881  }
882 
883  double mean = 0;
884  double *vd = v.data;
885  for (unsigned int i = 0; i < v.getCols(); i++)
886  mean += *(vd++);
887 
888  return mean / v.getCols();
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  std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);
901 
902  return vpMath::getMedian(vectorOfDoubles);
903 }
904 
908 double vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
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  double mean_value = mean(v);
915  double sum_squared_diff = 0.0;
916  for (unsigned int i = 0; i < v.size(); i++) {
917  sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
918  }
919 
920  double divisor = (double)v.size();
921  if (useBesselCorrection && v.size() > 1) {
922  divisor = divisor - 1;
923  }
924 
925  return std::sqrt(sum_squared_diff / divisor);
926 }
927 
947 int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) const
948 {
949  typedef std::string::size_type size_type;
950 
951  unsigned int m = 1;
952  unsigned int n = getCols();
953 
954  std::vector<std::string> values(m * n);
955  std::ostringstream oss;
956  std::ostringstream ossFixed;
957  std::ios_base::fmtflags original_flags = oss.flags();
958 
959  // ossFixed <<std::fixed;
960  ossFixed.setf(std::ios::fixed, std::ios::floatfield);
961 
962  size_type maxBefore = 0; // the length of the integral part
963  size_type maxAfter = 0; // number of decimals plus
964  // one place for the decimal point
965  for (unsigned int j = 0; j < n; ++j) {
966  oss.str("");
967  oss << (*this)[j];
968  if (oss.str().find("e") != std::string::npos) {
969  ossFixed.str("");
970  ossFixed << (*this)[j];
971  oss.str(ossFixed.str());
972  }
973 
974  values[j] = oss.str();
975  size_type thislen = values[j].size();
976  size_type p = values[j].find('.');
977 
978  if (p == std::string::npos) {
979  maxBefore = vpMath::maximum(maxBefore, thislen);
980  // maxAfter remains the same
981  } else {
982  maxBefore = vpMath::maximum(maxBefore, p);
983  maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
984  }
985  }
986 
987  size_type totalLength = length;
988  // increase totalLength according to maxBefore
989  totalLength = vpMath::maximum(totalLength, maxBefore);
990  // decrease maxAfter according to totalLength
991  maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
992  if (maxAfter == 1)
993  maxAfter = 0;
994 
995  // the following line is useful for debugging
996  // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
997 
998  if (intro)
999  s << intro;
1000  s << "[" << m << "," << n << "]=\n";
1001 
1002  s << " ";
1003  for (unsigned int j = 0; j < n; j++) {
1004  size_type p = values[j].find('.');
1005  s.setf(std::ios::right, std::ios::adjustfield);
1006  s.width((std::streamsize)maxBefore);
1007  s << values[j].substr(0, p).c_str();
1008 
1009  if (maxAfter > 0) {
1010  s.setf(std::ios::left, std::ios::adjustfield);
1011  if (p != std::string::npos) {
1012  s.width((std::streamsize)maxAfter);
1013  s << values[j].substr(p, maxAfter).c_str();
1014  } else {
1015  assert(maxAfter > 1);
1016  s.width((std::streamsize)maxAfter);
1017  s << ".0";
1018  }
1019  }
1020 
1021  s << ' ';
1022  }
1023  s << std::endl;
1024 
1025  s.flags(original_flags); // restore s to standard state
1026 
1027  return (int)(maxBefore + maxAfter);
1028 }
1029 
1033 vpRowVector operator*(const double &x, const vpRowVector &v)
1034 {
1035  vpRowVector vout;
1036  vout = v * x;
1037  return vout;
1038 }
1039 
1045 double vpRowVector::sum() const
1046 {
1047  double sum = 0.0;
1048 
1049  for (unsigned int j = 0; j < colNum; j++) {
1050  sum += rowPtrs[0][j];
1051  }
1052 
1053  return sum;
1054 }
1055 
1063 {
1064  double sum_square = 0.0;
1065 
1066  for (unsigned int j = 0; j < colNum; j++) {
1067  double x = rowPtrs[0][j];
1068  sum_square += x * x;
1069  }
1070 
1071  return sum_square;
1072 }
1073 
1084 {
1085  return frobeniusNorm();
1086 }
1087 
1094 {
1095  double norm = sumSquare();
1096 
1097  return sqrt(norm);
1098 }
1099 
1135 void vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
1136 {
1137  unsigned int cncols = c + ncols;
1138 
1139  if (cncols > v.getCols())
1140  throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector",
1141  cncols, v.getCols()));
1142  resize(ncols);
1143  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
1144  return; // Noting to do
1145  for (unsigned int i = 0; i < ncols; i++)
1146  (*this)[i] = v[i + c];
1147 }
1148 
1179 std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
1180 {
1181  os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl;
1182 
1183  for (unsigned int j = 0; j < this->getCols(); ++j) {
1184  if (!octet) {
1185  os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl;
1186  } else {
1187  for (unsigned int k = 0; k < sizeof(double); ++k) {
1188  os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex
1189  << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl;
1190  }
1191  }
1192  }
1193  std::cout << std::endl;
1194  return os;
1195 }
1196 
1221 std::ostream &vpRowVector::csvPrint(std::ostream &os) const
1222 {
1223  for (unsigned int j = 0; j < this->getCols(); ++j) {
1224  os << (*this)[j];
1225  if (!(j == (this->getCols() - 1)))
1226  os << ", ";
1227  }
1228  os << std::endl;
1229  return os;
1230 }
1231 
1255 std::ostream &vpRowVector::maplePrint(std::ostream &os) const
1256 {
1257  os << "([ " << std::endl;
1258  os << "[";
1259  for (unsigned int j = 0; j < this->getCols(); ++j) {
1260  os << (*this)[j] << ", ";
1261  }
1262  os << "]," << std::endl;
1263  os << "])" << std::endl;
1264  return os;
1265 }
1266 
1297 std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
1298 {
1299  os << "[ ";
1300  for (unsigned int j = 0; j < this->getCols(); ++j) {
1301  os << (*this)[j] << ", ";
1302  }
1303  os << "]" << std::endl;
1304  return os;
1305 }
vpRowVector & operator/=(double x)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRowVector & normalize()
static double median(const vpRowVector &v)
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::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
std::vector< double > toStdVector()
error that can be emited by ViSP classes.
Definition: vpException.h:71
static double mean(const vpRowVector &v)
vpRowVector operator+(const vpRowVector &v) const
vp_deprecated void init()
Definition: vpRowVector.h:318
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
unsigned int getCols() const
Definition: vpArray2D.h:279
double sum() const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
vpColVector transpose() const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpRowVector operator/(const double x) const
std::ostream & matlabPrint(std::ostream &os) const
vpColVector t() const
vp_deprecated double euclideanNorm() const
double operator*(const vpColVector &x) const
vpRowVector & operator-=(vpRowVector v)
void stack(double d)
unsigned int getRows() const
Definition: vpArray2D.h:289
double frobeniusNorm() const
std::ostream & maplePrint(std::ostream &os) const
vpRowVector operator-() const
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:268
vpRowVector & operator+=(vpRowVector v)
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
std::ostream & csvPrint(std::ostream &os) const
vpRowVector & operator<<(const vpRowVector &v)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpRowVector & operator*=(double x)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
vpRowVector & operator=(const vpRowVector &v)
Copy operator. Allow operation such as A = v.
Definition: vpRowVector.cpp:58
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
double sumSquare() 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)