Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRowVector.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
41#include <assert.h>
42#include <cmath>
43#include <sstream>
44#include <stdlib.h>
45#include <string.h>
46
47#include <visp3/core/vpArray2D.h>
48#include <visp3/core/vpColVector.h>
49#include <visp3/core/vpDebug.h>
50#include <visp3/core/vpException.h>
51#include <visp3/core/vpMatrix.h>
52#include <visp3/core/vpRowVector.h>
53
56{
57 unsigned int k = v.colNum;
58 if (colNum != k) {
59 try {
60 resize(k);
61 }
62 catch (...) {
63 throw;
64 }
65 }
66
67 memcpy(data, v.data, colNum * sizeof(double));
68
69 return *this;
70}
71
80{
81 if (M.getRows() != 1) {
82 throw(vpException(vpException::dimensionError, "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
83 M.getCols(), M.getRows(), M.getCols()));
84 }
85
86 if (M.getCols() != colNum)
87 resize(M.getCols());
88
89 memcpy(data, M.data, colNum * sizeof(double));
90 return *this;
91}
92
96vpRowVector &vpRowVector::operator=(const std::vector<double> &v)
97{
98 resize((unsigned int)v.size());
99 for (unsigned int i = 0; i < v.size(); i++)
100 (*this)[i] = v[i];
101 return *this;
102}
106vpRowVector &vpRowVector::operator=(const std::vector<float> &v)
107{
108 resize((unsigned int)v.size());
109 for (unsigned int i = 0; i < v.size(); i++)
110 (*this)[i] = (float)v[i];
111 return *this;
112}
113
116{
117 for (unsigned int i = 0; i < rowNum; i++) {
118 for (unsigned int j = 0; j < colNum; j++) {
119 rowPtrs[i][j] = x;
120 }
121 }
122 return *this;
123}
124
125#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
127{
128 if (this != &other) {
129 free(data);
130 free(rowPtrs);
131
132 rowNum = other.rowNum;
133 colNum = other.colNum;
134 rowPtrs = other.rowPtrs;
135 dsize = other.dsize;
136 data = other.data;
137
138 other.rowNum = 0;
139 other.colNum = 0;
140 other.rowPtrs = NULL;
141 other.dsize = 0;
142 other.data = NULL;
143 }
144
145 return *this;
146}
147
166vpRowVector &vpRowVector::operator=(const std::initializer_list<double> &list)
167{
168 resize(1, static_cast<unsigned int>(list.size()), false);
169 std::copy(list.begin(), list.end(), data);
170 return *this;
171}
172#endif
173
175{
176 if (colNum != v.colNum || rowNum != v.rowNum /* should not happen */)
177 return false;
178
179 for (unsigned int i = 0; i < colNum; i++) {
180 if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon()))
181 return false;
182 }
183
184 return true;
185}
186
187bool vpRowVector::operator!=(const vpRowVector &v) const { return !(*this == v); }
188
204{
205 unsigned int nelements = x.getRows();
206 if (getCols() != nelements) {
207 throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum,
208 x.getRows()));
209 }
210
211 double scalar = 0.0;
212
213 for (unsigned int i = 0; i < nelements; i++) {
214 scalar += (*this)[i] * x[i];
215 }
216 return scalar;
217}
234{
235 vpRowVector c(M.getCols());
236
237 if (colNum != M.getRows()) {
238 throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum,
239 M.getRows(), M.getCols()));
240 }
241
242 c = 0.0;
243
244 for (unsigned int i = 0; i < colNum; i++) {
245 double bi = data[i]; // optimization em 5/12/2006
246 for (unsigned int j = 0; j < M.getCols(); j++) {
247 c[j] += bi * M[i][j];
248 }
249 }
250
251 return c;
252}
253
274{
276
277 double *vd = v.data;
278 double *d = data;
279
280 for (unsigned int i = 0; i < colNum; i++)
281 *(vd++) = (*d++) * x;
282 return v;
283}
284
303{
304 for (unsigned int i = 0; i < colNum; i++)
305 (*this)[i] *= x;
306 return (*this);
307}
308
329{
331
332 double *vd = v.data;
333 double *d = data;
334
335 for (unsigned int i = 0; i < colNum; i++)
336 *(vd++) = (*d++) / x;
337 return v;
338}
339
359{
360 for (unsigned int i = 0; i < colNum; i++)
361 (*this)[i] /= x;
362 return (*this);
363}
364
376{
378
379 double *vd = A.data;
380 double *d = data;
381
382 for (unsigned int i = 0; i < colNum; i++)
383 *(vd++) = -(*d++);
384
385 return A;
386}
387
393{
394 if (getCols() != m.getCols()) {
395 throw(vpException(vpException::dimensionError, "Cannot subtract (1x%d) row vector to (1x%d) row vector", getCols(),
396 m.getCols()));
397 }
398
400
401 for (unsigned int i = 0; i < colNum; i++)
402 v[i] = (*this)[i] - m[i];
403 return v;
404}
405
411{
412 if (getCols() != v.getCols()) {
413 throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
414 v.getCols()));
415 }
416
418
419 for (unsigned int i = 0; i < colNum; i++)
420 r[i] = (*this)[i] + v[i];
421 return r;
422}
423
430{
431 if (getCols() != v.getCols()) {
432 throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
433 v.getCols()));
434 }
435
436 for (unsigned int i = 0; i < colNum; i++)
437 (*this)[i] += v[i];
438 return (*this);
439}
440
447{
448 if (getCols() != v.getCols()) {
449 throw(vpException(vpException::dimensionError, "Cannot subtract (1x%d) row vector to (1x%d) row vector", getCols(),
450 v.getCols()));
451 }
452
453 for (unsigned int i = 0; i < colNum; i++)
454 (*this)[i] -= v[i];
455 return (*this);
456}
457
480{
481 *this = v;
482 return *this;
483}
484
486{
487 resize(1, false);
488 data[0] = val;
489 return *this;
490}
491
493{
494 resize(colNum + 1, false);
495 data[colNum - 1] = val;
496 return *this;
497}
498
503{
505 memcpy(v.data, data, colNum * sizeof(double));
506 return v;
507}
508
518void vpRowVector::transpose(vpColVector &v) const { v = t(); }
519
524vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols())
525{
526 for (unsigned int j = 0; j < M.getCols(); j++)
527 (*this)[j] = M[i][j];
528}
535vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D<double>(1, M.getCols())
536{
537 if (M.getRows() != 1) {
538 throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
539 M.getCols(), M.getRows(), M.getCols()));
540 }
541
542 for (unsigned int j = 0; j < M.getCols(); j++)
543 (*this)[j] = M[0][j];
544}
545
549vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, (unsigned int)v.size())
550{
551 for (unsigned int j = 0; j < v.size(); j++)
552 (*this)[j] = v[j];
553}
557vpRowVector::vpRowVector(const std::vector<float> &v) : vpArray2D<double>(1, (unsigned int)v.size())
558{
559 for (unsigned int j = 0; j < v.size(); j++)
560 (*this)[j] = (double)(v[j]);
561}
562
576vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
577{
578 init(v, c, ncols);
579}
580
581#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
583{
584 rowNum = v.rowNum;
585 colNum = v.colNum;
586 rowPtrs = v.rowPtrs;
587 dsize = v.dsize;
588 data = v.data;
589
590 v.rowNum = 0;
591 v.colNum = 0;
592 v.rowPtrs = NULL;
593 v.dsize = 0;
594 v.data = NULL;
595}
596#endif
597
608{
609 x = x / sqrt(x.sumSquare());
610
611 return x;
612}
613
623{
624 double sum_square = sumSquare();
625 if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
626 *this /= sqrt(sum_square);
627 }
628
629 // If sum = 0, we have a nul vector. So we return just.
630 return *this;
631}
632
644vpMatrix vpRowVector::reshape(unsigned int nrows, unsigned int ncols)
645{
646 vpMatrix M(nrows, ncols);
647 reshape(M, nrows, ncols);
648 return M;
649}
650
694void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
695{
696 if (dsize != nrows * ncols) {
697 throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum,
698 M.getRows(), M.getCols()));
699 }
700 try {
701 if ((M.getRows() != nrows) || (M.getCols() != ncols))
702 M.resize(nrows, ncols);
703 }
704 catch (...) {
705 throw;
706 }
707 for (unsigned int i = 0; i < nrows; i++)
708 for (unsigned int j = 0; j < ncols; j++)
709 M[i][j] = data[i * ncols + j];
710}
711
743void vpRowVector::insert(unsigned int i, const vpRowVector &v)
744{
745 if (i + v.size() > this->size())
747 "Unable to insert (1x%d) row vector in (1x%d) row "
748 "vector at position (%d)",
749 v.getCols(), colNum, i));
750 for (unsigned int j = 0; j < v.size(); j++)
751 (*this)[i + j] = v[j];
752}
753
758std::vector<double> vpRowVector::toStdVector() const
759{
760 std::vector<double> v(this->size());
761
762 for (unsigned int i = 0; i < this->size(); i++)
763 v[i] = data[i];
764 return v;
765}
766
783void vpRowVector::stack(double d)
784{
785 this->resize(colNum + 1, false);
786 (*this)[colNum - 1] = d;
787}
788
808void vpRowVector::stack(const vpRowVector &v) { *this = vpRowVector::stack(*this, v); }
809
831{
832 vpRowVector C;
833 vpRowVector::stack(A, B, C);
834 return C;
835}
836
858{
859 unsigned int nrA = A.getCols();
860 unsigned int nrB = B.getCols();
861
862 if (nrA == 0 && nrB == 0) {
863 C.resize(0);
864 return;
865 }
866
867 if (nrB == 0) {
868 C = A;
869 return;
870 }
871
872 if (nrA == 0) {
873 C = B;
874 return;
875 }
876
877 // General case
878 C.resize(nrA + nrB);
879
880 for (unsigned int i = 0; i < nrA; i++)
881 C[i] = A[i];
882
883 for (unsigned int i = 0; i < nrB; i++)
884 C[nrA + i] = B[i];
885}
886
891{
892 if (v.data == NULL || v.size() == 0) {
893 throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
894 }
895
896 double mean = 0;
897 double *vd = v.data;
898 for (unsigned int i = 0; i < v.getCols(); i++)
899 mean += *(vd++);
900
901 return mean / v.getCols();
902}
903
908{
909 if (v.data == NULL || v.size() == 0) {
910 throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
911 }
912
913 std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);
914
915 return vpMath::getMedian(vectorOfDoubles);
916}
917
921double vpRowVector::stdev(const vpRowVector &v, bool useBesselCorrection)
922{
923 if (v.data == NULL || v.size() == 0) {
924 throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
925 }
926
927 double mean_value = mean(v);
928 double sum_squared_diff = 0.0;
929 for (unsigned int i = 0; i < v.size(); i++) {
930 sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
931 }
932
933 double divisor = (double)v.size();
934 if (useBesselCorrection && v.size() > 1) {
935 divisor = divisor - 1;
936 }
937
938 return std::sqrt(sum_squared_diff / divisor);
939}
940
960int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) const
961{
962 typedef std::string::size_type size_type;
963
964 unsigned int m = 1;
965 unsigned int n = getCols();
966
967 std::vector<std::string> values(m * n);
968 std::ostringstream oss;
969 std::ostringstream ossFixed;
970 std::ios_base::fmtflags original_flags = oss.flags();
971
972 // ossFixed <<std::fixed;
973 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
974
975 size_type maxBefore = 0; // the length of the integral part
976 size_type maxAfter = 0; // number of decimals plus
977 // one place for the decimal point
978 for (unsigned int j = 0; j < n; ++j) {
979 oss.str("");
980 oss << (*this)[j];
981 if (oss.str().find("e") != std::string::npos) {
982 ossFixed.str("");
983 ossFixed << (*this)[j];
984 oss.str(ossFixed.str());
985 }
986
987 values[j] = oss.str();
988 size_type thislen = values[j].size();
989 size_type p = values[j].find('.');
990
991 if (p == std::string::npos) {
992 maxBefore = vpMath::maximum(maxBefore, thislen);
993 // maxAfter remains the same
994 }
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 }
1029 else {
1030 assert(maxAfter > 1);
1031 s.width((std::streamsize)maxAfter);
1032 s << ".0";
1033 }
1034 }
1035
1036 s << ' ';
1037 }
1038 s << std::endl;
1039
1040 s.flags(original_flags); // restore s to standard state
1041
1042 return (int)(maxBefore + maxAfter);
1043}
1044
1048vpRowVector operator*(const double &x, const vpRowVector &v)
1049{
1050 vpRowVector vout;
1051 vout = v * x;
1052 return vout;
1053}
1054
1060double vpRowVector::sum() const
1061{
1062 double sum = 0.0;
1063
1064 for (unsigned int j = 0; j < colNum; j++) {
1065 sum += rowPtrs[0][j];
1066 }
1067
1068 return sum;
1069}
1070
1078{
1079 double sum_square = 0.0;
1080
1081 for (unsigned int j = 0; j < colNum; j++) {
1082 double x = rowPtrs[0][j];
1083 sum_square += x * x;
1084 }
1085
1086 return sum_square;
1087}
1088
1095{
1096 double norm = sumSquare();
1097
1098 return sqrt(norm);
1099}
1100
1101#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1111double vpRowVector::euclideanNorm() const { return frobeniusNorm(); }
1112#endif
1113
1149void vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
1150{
1151 unsigned int cncols = c + ncols;
1152
1153 if (cncols > v.getCols())
1154 throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector",
1155 cncols, v.getCols()));
1156 resize(ncols);
1157 if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
1158 return; // Noting to do
1159 for (unsigned int i = 0; i < ncols; i++)
1160 (*this)[i] = v[i + c];
1161}
1162
1193std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixName, bool octet) const
1194{
1195 os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl;
1196
1197 for (unsigned int j = 0; j < this->getCols(); ++j) {
1198 if (!octet) {
1199 os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl;
1200 }
1201 else {
1202 for (unsigned int k = 0; k < sizeof(double); ++k) {
1203 os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex
1204 << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl;
1205 }
1206 }
1207 }
1208 std::cout << std::endl;
1209 return os;
1210}
1211
1236std::ostream &vpRowVector::csvPrint(std::ostream &os) const
1237{
1238 for (unsigned int j = 0; j < this->getCols(); ++j) {
1239 os << (*this)[j];
1240 if (!(j == (this->getCols() - 1)))
1241 os << ", ";
1242 }
1243 os << std::endl;
1244 return os;
1245}
1246
1270std::ostream &vpRowVector::maplePrint(std::ostream &os) const
1271{
1272 os << "([ " << std::endl;
1273 os << "[";
1274 for (unsigned int j = 0; j < this->getCols(); ++j) {
1275 os << (*this)[j] << ", ";
1276 }
1277 os << "]," << std::endl;
1278 os << "])" << std::endl;
1279 return os;
1280}
1281
1312std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
1313{
1314 os << "[ ";
1315 for (unsigned int j = 0; j < this->getCols(); ++j) {
1316 os << (*this)[j] << ", ";
1317 }
1318 os << "]" << std::endl;
1319 return os;
1320}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int rowNum
Definition vpArray2D.h:134
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
Definition vpArray2D.h:529
unsigned int dsize
Definition vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
unsigned int colNum
Number of columns in the array.
Definition vpArray2D.h:136
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of row vector and the associated operations.
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)
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.
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.
vp_deprecated void init()
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
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
vpColVector operator*(const double &x, const vpColVector &v)