55 #include <visp3/core/vpColVector.h>
56 #include <visp3/core/vpException.h>
57 #include <visp3/core/vpMath.h>
58 #include <visp3/core/vpDebug.h>
59 #include <visp3/core/vpRotationVector.h>
61 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
62 # include <emmintrin.h>
63 # define VISP_HAVE_SSE2 1
73 "Cannot add (%dx1) column vector to (%dx1) column vector",
78 for (
unsigned int i=0;i<
rowNum;i++)
79 r[i] = (*
this)[i] + v[i];
105 "Cannot add %d-dimension column vector to a translation vector",
getRows()));
109 for (
unsigned int i=0;i<3;i++) s[i] = (*
this)[i]+t[i] ;
120 "Cannot add (%dx1) column vector to (%dx1) column vector",
124 for (
unsigned int i=0;i<
rowNum;i++)
134 "Cannot substract (%dx1) column vector to (%dx1) column vector",
138 for (
unsigned int i=0;i<
rowNum;i++)
156 "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
161 for (
unsigned int i=0;i<
rowNum;i++)
162 r += (*
this)[i] * v[i];
178 for (
unsigned int i=0; i<
rowNum; i++) {
179 for (
unsigned int j=0; j<v.
getCols(); j++) {
180 M[i][j] = (*this)[i] * v[j];
191 "Bad size during vpColVector (%dx1) and vpColVector (%dx1) substraction",
196 for (
unsigned int i=0;i<
rowNum;i++)
197 v[i] = (*
this)[i] - m[i];
257 unsigned int rnrows = r+nrows ;
261 "Bad row dimension (%d > %d) used to initialize vpColVector",
267 for (
unsigned int i=r ; i < rnrows; i++)
274 for (
unsigned int i=0; i< v.
size(); i++)
281 for (
unsigned int i=0; i< p.
size(); i++)
288 for (
unsigned int i=0; i< v.
size(); i++)
296 for(
unsigned int i=0; i< M.
getCols(); i++)
297 (*
this)[i] = M[i][j];
310 "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
314 for(
unsigned int i=0; i< M.
getRows(); i++)
315 (*
this)[i] = M[i][0];
322 :
vpArray2D<double>((unsigned int)v.size(), 1)
324 for(
unsigned int i=0; i< v.size(); i++)
331 :
vpArray2D<double>((unsigned int)v.size(), 1)
333 for(
unsigned int i=0; i< v.size(); i++)
334 (*
this)[i] = (double)(v[i]);
359 double *vd = A.
data ;
double *d =
data ;
361 for (
unsigned int i=0; i<
rowNum; i++)
390 double *vd = v.
data ;
double *d =
data ;
392 for (
unsigned int i=0;i<
rowNum;i++)
393 *(vd++) = (*d++) * x;
416 for (
unsigned int i=0;i<
rowNum;i++)
440 for (
unsigned int i=0;i<
rowNum;i++)
468 double *vd = v.
data ;
double *d =
data ;
470 for (
unsigned int i=0;i<
rowNum;i++)
471 *(vd++) = (*d++) / x;
484 "Cannot transform a (%dx%d) matrix into a column vector",
505 resize((
unsigned int)v.size());
506 for(
unsigned int i=0; i<v.size(); i++)
515 resize((
unsigned int)v.size());
516 for(
unsigned int i=0; i<v.size(); i++)
517 (*
this)[i] = (float)v[i];
523 unsigned int k = v.
rowNum ;
543 unsigned int k = tv.
getRows() ;
562 unsigned int k = rv.
getRows() ;
648 for (
unsigned int i=0; i<
rowNum; i++) {
649 for (
unsigned int j=0; j<
colNum; j++) {
661 for (
unsigned int i=0;i<
rowNum;i++)
717 "Cannot compute the dot product: first vector empty")) ;
721 "Cannot compute the dot product: second vector empty")) ;
725 "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
729 double *ad = a.
data ;
double *bd = b.
data ;
732 for (
unsigned int i=0 ; i < a.
getRows() ; i++)
733 c += *(ad++)* *(bd++) ;
767 if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
768 *
this /= sqrt(sum_square) ;
783 "Cannot sort content of column vector: vector empty")) ;
787 unsigned int nb_permutation = 1 ;
789 while (nb_permutation !=0 )
792 for (
unsigned int j = v.
getRows()-1 ; j >= i+1 ; j--)
794 if ((tab[j]>tab[j-1]))
796 double tmp = tab[j] ;
817 "Cannot sort content of column vector: vector empty")) ;
821 unsigned int nb_permutation = 1 ;
823 while (nb_permutation !=0 )
826 for (
unsigned int j = v.
getRows()-1 ; j >= i+1 ; j--)
828 if ((tab[j]<tab[j-1]))
830 double tmp = tab[j] ;
933 unsigned int nrA = A.
getRows();
934 unsigned int nrB = B.
getRows();
936 if (nrA == 0 && nrB == 0) {
954 for (
unsigned int i=0; i<nrA; i++)
957 for (
unsigned int i=0; i<nrB; i++)
968 "Cannot compute column vector mean: vector empty")) ;
990 "Cannot compute column vector median: vector empty")) ;
993 std::vector<double> vectorOfDoubles(v.
size());
994 for(
unsigned int i = 0; i < v.
size(); i++) {
995 vectorOfDoubles[i] = v[i];
1009 "Cannot compute column vector stdev: vector empty")) ;
1012 double mean_value =
mean(v);
1013 double sum_squared_diff = 0.0;
1017 __m128d v_sub, v_mul, v_sum = _mm_setzero_pd();
1025 __m128d v_mean = _mm_set_pd(mean_value, mean_value);
1028 for(; i <= v.
getRows()- 4; i+=4) {
1029 v_sub = _mm_sub_pd(_mm_loadu_pd(v.
data + i), v_mean);
1030 v_mul = _mm_mul_pd(v_sub, v_sub);
1031 v_sum = _mm_add_pd(v_mul, v_sum);
1033 v_sub = _mm_sub_pd(_mm_loadu_pd(v.
data + i + 2), v_mean);
1034 v_mul = _mm_mul_pd(v_sub, v_sub);
1035 v_sum = _mm_add_pd(v_mul, v_sum);
1040 _mm_storeu_pd(res, v_sum);
1042 sum_squared_diff = res[0]+res[1];
1052 sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
1055 double divisor = (double) v.
size();
1056 if(useBesselCorrection && v.
size() > 1) {
1057 divisor = divisor-1;
1060 return std::sqrt(sum_squared_diff / divisor);
1083 "Cannot compute skew vector of a non 3-dimention vector (%d)",
1088 M[0][0] = 0 ; M[0][1] = -v[2] ; M[0][2] = v[1] ;
1089 M[1][0] = v[2] ; M[1][1] = 0 ; M[1][2] = -v[0] ;
1090 M[2][0] = -v[1] ; M[2][1] = v[0] ; M[2][2] = 0 ;
1108 "Cannot compute the cross product between column vector with dimension %d and %d",
1187 if(
dsize!=nrows*ncols) {
1189 "Cannot reshape (%dx1) column vector in (%dx%d) matrix",
1199 for(
unsigned int j =0; j< ncols; j++)
1200 for(
unsigned int i =0; i< nrows; i++)
1201 M[i][j]=
data[j*nrows+i];
1240 for (
unsigned int j=0; j < v.
size(); j++)
1241 (*
this)[i+j] = v[j];
1266 typedef std::string::size_type size_type;
1271 std::vector<std::string> values(m*n);
1272 std::ostringstream oss;
1273 std::ostringstream ossFixed;
1274 std::ios_base::fmtflags original_flags = oss.flags();
1277 ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
1279 size_type maxBefore=0;
1280 size_type maxAfter=0;
1282 for (
unsigned int i=0;i<m;++i) {
1285 if (oss.str().find(
"e")!=std::string::npos){
1287 ossFixed << (*this)[i];
1288 oss.str(ossFixed.str());
1291 values[i]=oss.str();
1292 size_type thislen=values[i].size();
1293 size_type p=values[i].find(
'.');
1295 if (p==std::string::npos){
1305 size_type totalLength=length;
1309 maxAfter=std::min(maxAfter, totalLength-maxBefore);
1310 if (maxAfter==1) maxAfter=0;
1315 if (intro) s <<intro;
1316 s <<
"["<<m<<
","<<n<<
"]=\n";
1318 for (
unsigned int i=0;i<m;i++) {
1320 size_type p=values[i].find(
'.');
1321 s.setf(std::ios::right, std::ios::adjustfield);
1322 s.width((std::streamsize)maxBefore);
1323 s <<values[i].substr(0,p).c_str();
1326 s.setf(std::ios::left, std::ios::adjustfield);
1327 if (p!=std::string::npos){
1328 s.width((std::streamsize)maxAfter);
1329 s <<values[i].substr(p,maxAfter).c_str();
1332 s.width((std::streamsize)maxAfter);
1342 s.flags(original_flags);
1344 return (
int)(maxBefore+maxAfter);
1358 __m128d v_sum1 = _mm_setzero_pd(), v_sum2 = _mm_setzero_pd(), v_sum;
1361 for(; i <=
rowNum- 4; i+=4) {
1362 v_sum1 = _mm_add_pd(_mm_loadu_pd(
data + i), v_sum1);
1363 v_sum2 = _mm_add_pd(_mm_loadu_pd(
data + i + 2), v_sum2);
1367 v_sum = _mm_add_pd(v_sum1, v_sum2);
1370 _mm_storeu_pd(res, v_sum);
1372 sum = res[0]+res[1];
1395 double sum_square = 0.0;
1399 __m128d v_mul1, v_mul2;
1400 __m128d v_sum = _mm_setzero_pd();
1403 for(; i <=
rowNum- 4; i+=4) {
1404 v_mul1 = _mm_mul_pd(_mm_loadu_pd(
data + i), _mm_loadu_pd(
data + i));
1405 v_mul2 = _mm_mul_pd(_mm_loadu_pd(
data + i + 2), _mm_loadu_pd(
data + i + 2));
1407 v_sum = _mm_add_pd(v_mul1, v_sum);
1408 v_sum = _mm_add_pd(v_mul2, v_sum);
1413 _mm_storeu_pd(res, v_sum);
1415 sum_square = res[0]+res[1];
1426 sum_square += (*this)[i] * (*this)[i];
1464 for (
unsigned int i=0;i<
rowNum;i++){
1465 double x = fabs ( (*
this)[i] ) ;
1504 os <<
"vpColVector " << matrixName
1505 <<
" (" <<
this ->getRows () <<
"); " <<std::endl;
1507 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
1510 os << matrixName <<
"[" << i <<
"] = " << (*this)[i] <<
"; " << std::endl;
1513 for (
unsigned int k = 0; k <
sizeof(double); ++ k) {
1514 os <<
"((unsigned char*)&(" << matrixName
1515 <<
"[" << i <<
"]) )[" << k
1516 <<
"] = 0x" <<std::hex<<
1517 (
unsigned int)((
unsigned char*)& ((*this)[i])) [k]
1518 <<
"; " << std::endl;
1522 std::cout << std::endl;
1554 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
1589 os <<
"([ " << std::endl;
1590 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
1592 os << (*this)[i] <<
", ";
1593 os <<
"]," << std::endl;
1595 os <<
"])" << std::endl;
1638 for (
unsigned int i=0; i < this->
getRows(); ++ i) {
1639 os << (*this)[i] <<
", ";
1640 if (
this ->
getRows() != i+1) { os <<
";" << std::endl; }
1641 else { os <<
"]" << std::endl; }
1646 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1665 #endif // defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Implementation of a generic rotation vector.
vp_deprecated void init()
static vpColVector invSort(const vpColVector &v)
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
static vpColVector sort(const vpColVector &v)
static double stdev(const vpColVector &v, const bool useBesselCorrection=false)
double euclideanNorm() const
Implementation of row vector and the associated operations.
std::ostream & matlabPrint(std::ostream &os) const
static double getMedian(const std::vector< double > &v)
vpColVector operator*(const double &x, const vpColVector &v)
vpColVector operator/(const double x) const
vpColVector operator+(const vpColVector &v) const
Operator that allows to add two column vectors.
error that can be emited by ViSP classes.
std::ostream & csvPrint(std::ostream &os) const
Type * data
Address of the first element of the data array.
Implementation of a generic 2D array used as vase class of matrices and vectors.
vpColVector & operator*=(double x)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getCols() const
Return the number of columns of the 2D array.
vpColVector operator-() const
static double median(const vpColVector &v)
static Type maximum(const Type &a, const Type &b)
vpColVector & operator/=(double x)
int print(std::ostream &s, unsigned int length, char const *intro=0) const
unsigned int rowNum
Number of rows in the array.
double infinityNorm() const
vpColVector & normalize()
std::ostream & maplePrint(std::ostream &os) const
std::ostream & cppPrint(std::ostream &os, const std::string &matrixName="A", bool octet=false) const
vpColVector & operator<<(const vpColVector &v)
static double mean(const vpColVector &v)
unsigned int getRows() const
Return the number of rows of the 2D array.
vpColVector & operator-=(vpColVector v)
Operator that allows to substract two column vectors.
unsigned int colNum
Number of columns in the array.
void insert(unsigned int i, const vpColVector &v)
vpColVector & operator=(const vpColVector &v)
Copy operator. Allow operation such as A = v.
vpColVector & operator+=(vpColVector v)
Operator that allows to add two column vectors.
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
vpRowVector transpose() const
Implementation of a pose vector and operations on poses.
vpColVector()
Basic constructor that creates an empty 0-size column vector.
static vpMatrix skew(const vpColVector &v)
unsigned int dsize
Current array size (rowNum * colNum)
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
double operator*(const vpColVector &x) const
Class that consider the case of a translation vector.
double ** rowPtrs
Address of the first element of each rows.
void resize(const unsigned int i, const bool flagNullify=true)