Visual Servoing Platform version 3.5.0
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
98vpRowVector &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}
108vpRowVector &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
169vpRowVector &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
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
191 return !(*this == v);
192}
193
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{
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{
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{
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 subtract (1x%d) row vector to (1x%d) row vector", getCols(),
401 m.getCols()));
402 }
403
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
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 subtract (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{
510 memcpy(v.data, data, colNum * sizeof(double));
511 return v;
512}
513
523void vpRowVector::transpose(vpColVector &v) const { v = t(); }
524
529vpRowVector::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}
540vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D<double>(1, M.getCols())
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
554vpRowVector::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}
562vpRowVector::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
581vpRowVector::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
649vpMatrix vpRowVector::reshape(unsigned int nrows, unsigned int ncols)
650{
651 vpMatrix M(nrows, ncols);
652 reshape(M, nrows, ncols);
653 return M;
654}
655
699void 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
747void 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
762std::vector<double> vpRowVector::toStdVector() const
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
787void vpRowVector::stack(double d)
788{
789 this->resize(colNum + 1, false);
790 (*this)[colNum - 1] = d;
791}
792
812void 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
925double vpRowVector::stdev(const vpRowVector &v, 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
964int 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
1050vpRowVector operator*(const double &x, const vpRowVector &v)
1051{
1052 vpRowVector vout;
1053 vout = v * x;
1054 return vout;
1055}
1056
1062double 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
1152void 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
1196std::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
1238std::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
1272std::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
1314std::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}
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:279
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:304
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:291
unsigned int getRows() const
Definition: vpArray2D.h:289
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:261
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
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:321
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