vpRowVector.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Operation on row vectors.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <string.h>
#include <stdlib.h>
#include <sstream>
#include <assert.h>
#include <cmath>

#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpRowVector.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>

vpRowVector & vpRowVector::operator=(const vpRowVector &v)
{
  unsigned int k = v.colNum ;
  if (colNum != k){
    try {
      resize(k);
    }
    catch(...)
    {
      throw;
    }
  }

  memcpy(data, v.data, colNum*sizeof(double)) ;

  return *this;
}

vpRowVector & vpRowVector::operator=(const vpMatrix &M)
{
  if (M.getRows() != 1 ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
                      M.getCols(), M.getRows(), M.getCols())) ;
  }

  if (M.getCols() != colNum)
    resize(M.getCols());

  memcpy(data, M.data, colNum*sizeof(double)) ;
  return *this;
}

vpRowVector & vpRowVector::operator=(const std::vector<double> &v)
{
  resize((unsigned int)v.size());
  for(unsigned int i=0; i<v.size(); i++)
    (*this)[i] = v[i];
  return *this;
}
vpRowVector & vpRowVector::operator=(const std::vector<float> &v)
{
  resize((unsigned int)v.size());
  for(unsigned int i=0; i<v.size(); i++)
    (*this)[i] = (float)v[i];
  return *this;
}

vpRowVector & vpRowVector::operator=(double x)
{
  for (unsigned int i=0; i<rowNum; i++) {
    for (unsigned int j=0; j<colNum; j++) {
      rowPtrs[i][j] = x;
    }
  }
  return *this;
}

double vpRowVector::operator*(const vpColVector &x) const
{
  unsigned int nelements = x.getRows();
  if (getCols() != nelements ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (1x%d) row vector by (%dx1) column vector",
                      colNum, x.getRows())) ;
  }

  double scalar = 0.0;

  for (unsigned int i=0; i<nelements; i++) {
    scalar += (*this)[i] * x[i];
  }
  return scalar;
}
vpRowVector vpRowVector::operator*(const vpMatrix &M) const
{
  vpRowVector c(M.getCols());

  if (colNum != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (1x%d) row vector by (%dx%d) matrix",
                      colNum, M.getRows(), M.getCols())) ;
  }

  c = 0.0;

  for (unsigned int i=0;i<colNum;i++) {
    double bi = data[i] ; // optimization em 5/12/2006
    for (unsigned int j=0;j<M.getCols();j++) {
      c[j]+=bi*M[i][j];
    }
  }

  return c ;
}

vpRowVector vpRowVector::operator*(double x) const
{
  vpRowVector v(colNum);

  double *vd = v.data ;   double *d = data ;

  for (unsigned int i=0;i<colNum;i++)
    *(vd++) = (*d++) * x;
  return v;
}

vpRowVector &vpRowVector::operator*=(double x)
{
  for (unsigned int i=0;i<colNum;i++)
    (*this)[i] *= x;
  return (*this);
}

vpRowVector vpRowVector::operator/(double x) const
{
  vpRowVector v(colNum);

  double *vd = v.data ;   double *d = data ;

  for (unsigned int i=0;i<colNum;i++)
    *(vd++) = (*d++) / x;
  return v;
}

vpRowVector &vpRowVector::operator/=(double x)
{
  for (unsigned int i=0;i<colNum;i++)
    (*this)[i] /= x;
  return (*this);
}

vpRowVector vpRowVector::operator-() const
{
  vpRowVector A(colNum);

  double *vd = A.data ;   double *d = data ;

  for (unsigned int i=0; i<colNum; i++)
    *(vd++)= - (*d++);

  return A;
}

vpRowVector vpRowVector::operator-(const vpRowVector &m) const
{
  if (getCols() != m.getCols() ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot substract (1x%d) row vector to (1x%d) row vector",
                      getCols(), m.getCols())) ;
  }

  vpRowVector v(colNum) ;

  for (unsigned int i=0;i<colNum;i++)
    v[i] = (*this)[i] - m[i];
  return v;
}

vpRowVector vpRowVector::operator+(const vpRowVector &v) const
{
  if (getCols() != v.getCols() ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (1x%d) row vector to (1x%d) row vector",
                      getCols(), v.getCols())) ;
  }

  vpRowVector r(colNum) ;

  for (unsigned int i=0;i<colNum;i++)
    r[i] = (*this)[i] + v[i];
  return r;
}

vpRowVector &
vpRowVector::operator+=(vpRowVector v)
{
  if (getCols() != v.getCols() ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (1x%d) row vector to (1x%d) row vector",
                      getCols(), v.getCols())) ;
  }

  for (unsigned int i=0;i<colNum;i++)
    (*this)[i] += v[i];
  return (*this);
}

vpRowVector &
vpRowVector::operator-=(vpRowVector v)
{
  if (getCols() != v.getCols() ) {
    throw(vpException(vpException::dimensionError,
                      "Cannot substract (1x%d) row vector to (1x%d) row vector",
                      getCols(), v.getCols())) ;
  }

  for (unsigned int i=0;i<colNum;i++)
    (*this)[i] -= v[i];
  return (*this);
}

vpRowVector & vpRowVector::operator<<(const vpRowVector &v)
{
  *this = v;
  return *this;
}

vpColVector vpRowVector::t() const
{
  vpColVector v(colNum);
  memcpy(v.data, data, colNum*sizeof(double)) ;
  return v;
}

vpColVector vpRowVector::transpose() const
{
  return t();
}
void vpRowVector::transpose(vpColVector &v) const
{
  v = t();
}

vpRowVector::vpRowVector (const vpMatrix &M, unsigned int i)
  : vpArray2D<double>(1, M.getCols())
{
  for(unsigned int j=0; j< M.getCols(); j++)
    (*this)[j] = M[i][j];
}
vpRowVector::vpRowVector (const vpMatrix &M)
  : vpArray2D<double>(1, M.getCols())
{
  if(M.getRows()!=1) {
    throw(vpException(vpException::dimensionError,
                      "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
                      M.getCols(), M.getRows(), M.getCols())) ;
  }

  for(unsigned int j=0; j< M.getCols(); j++)
    (*this)[j] = M[0][j];
}

vpRowVector::vpRowVector (const std::vector<double> &v)
  : vpArray2D<double>(1, (unsigned int)v.size())
{
  for(unsigned int j=0; j< v.size(); j++)
    (*this)[j] = v[j];
}
vpRowVector::vpRowVector (const std::vector<float> &v)
  : vpArray2D<double>(1, (unsigned int)v.size())
{
  for(unsigned int j=0; j< v.size(); j++)
    (*this)[j] = (double)(v[j]);
}

vpRowVector::vpRowVector (const vpRowVector &v, unsigned int c, unsigned int ncols)
  : vpArray2D<double>(1, ncols)
{
  init(v, c, ncols);
}

vpRowVector &vpRowVector::normalize(vpRowVector &x) const
{
  x = x/sqrt(x.sumSquare());

  return x;
}


vpRowVector &vpRowVector::normalize()
{
  double sum_square = sumSquare();
  if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
    *this /= sqrt(sum_square) ;
  }

  // If sum = 0, we have a nul vector. So we return just.
  return *this;
}

vpMatrix vpRowVector::reshape(const unsigned int &nrows, const unsigned int &ncols)
{
  vpMatrix M(nrows,ncols);
  reshape(M, nrows, ncols);
  return M;
}

void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
{
  if(dsize!=nrows*ncols) {
    throw(vpException(vpException::dimensionError,
                      "Cannot reshape (1x%d) row vector in (%dx%d) matrix",
                      colNum, M.getRows(), M.getCols())) ;
  }
  try {
    if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
  }
  catch(...) {
    throw ;
  }
  for(unsigned int i =0; i< nrows; i++)
    for(unsigned int j =0; j< ncols; j++)
      M[i][j]=data[i*ncols+j];
}

void vpRowVector::insert(unsigned int i, const vpRowVector &v)
{
  if (i+v.size() > this->size())
    throw(vpException(vpException::dimensionError,
                      "Unable to insert (1x%d) row vector in (1x%d) row vector at position (%d)",
                      v.getCols(), colNum, i));
  for (unsigned int j=0; j < v.size(); j++)
    (*this)[i+j] = v[j];
}

void vpRowVector::stack(const double &d)
{
  this->resize(colNum+1,false);
  (*this)[colNum-1] = d;
}

void vpRowVector::stack(const vpRowVector &v)
{
  *this = vpRowVector::stack(*this, v);
}

vpRowVector vpRowVector::stack(const vpRowVector &A, const vpRowVector &B)
{
  vpRowVector C;
  vpRowVector::stack(A, B, C);
  return C;
}

void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector &C)
{
  unsigned int nrA = A.getCols();
  unsigned int nrB = B.getCols();

  if (nrA == 0 && nrB == 0) {
    C.resize(0);
    return;
  }

  if (nrB == 0) {
    C = A;
    return;
  }

  if (nrA == 0) {
    C = B;
    return;
  }

  // General case
  C.resize(nrA + nrB);

  for (unsigned int i=0; i<nrA; i++)
    C[i] = A[i];

  for (unsigned int i=0; i<nrB; i++)
    C[nrA+i] = B[i];
}

double vpRowVector::mean(const vpRowVector &v)
{
  if (v.data == NULL) {
    throw(vpException(vpException::fatalError,
                      "Cannot compute mean value of an empty row vector"));
  }

  double mean = 0;
  double *vd = v.data;
  for (unsigned int i = 0; i < v.getCols(); i++)
    mean += *(vd++);

  return mean / v.getCols();
}

double
vpRowVector::median(const vpRowVector &v)
{
  if (v.data==NULL) {
    throw(vpException(vpException::fatalError,
                      "Cannot compute mean value of an empty row vector"));
  }

  std::vector<double> vectorOfDoubles(v.size());
  for(unsigned int i = 0; i < v.size(); i++) {
    vectorOfDoubles[i] = v[i];
  }

  return vpMath::getMedian(vectorOfDoubles);
}

double
vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
{
  if (v.data==NULL) {
    throw(vpException(vpException::fatalError,
                      "Cannot compute mean value of an empty row vector"));
  }

  double mean_value = mean(v);
  double sum_squared_diff = 0.0;
  for(unsigned int i = 0; i < v.size(); i++) {
    sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
  }

  double divisor = (double) v.size();
  if(useBesselCorrection && v.size() > 1) {
    divisor = divisor-1;
  }

  return std::sqrt(sum_squared_diff / divisor);
}

int
vpRowVector::print(std::ostream& s, unsigned int length, char const* intro) const
{
  typedef std::string::size_type size_type;

  unsigned int m = 1;
  unsigned int n = getCols();

  std::vector<std::string> values(m*n);
  std::ostringstream oss;
  std::ostringstream ossFixed;
  std::ios_base::fmtflags original_flags = oss.flags();

  // ossFixed <<std::fixed;
  ossFixed.setf ( std::ios::fixed, std::ios::floatfield );

  size_type maxBefore=0;  // the length of the integral part
  size_type maxAfter=0;   // number of decimals plus
  // one place for the decimal point
  for (unsigned int j=0;j<n;++j){
    oss.str("");
    oss << (*this)[j];
    if (oss.str().find("e")!=std::string::npos){
      ossFixed.str("");
      ossFixed << (*this)[j];
      oss.str(ossFixed.str());
    }

    values[j]=oss.str();
    size_type thislen=values[j].size();
    size_type p=values[j].find('.');

    if (p==std::string::npos){
      maxBefore=vpMath::maximum(maxBefore, thislen);
      // maxAfter remains the same
    } else{
      maxBefore=vpMath::maximum(maxBefore, p);
      maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
    }
  }


  size_type totalLength=length;
  // increase totalLength according to maxBefore
  totalLength=vpMath::maximum(totalLength,maxBefore);
  // decrease maxAfter according to totalLength
  maxAfter=std::min(maxAfter, totalLength-maxBefore);
  if (maxAfter==1) maxAfter=0;

  // the following line is useful for debugging
  //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";

  if (intro) s <<intro;
  s <<"["<<m<<","<<n<<"]=\n";

  s <<"  ";
  for (unsigned int j=0;j<n;j++){
    size_type p=values[j].find('.');
    s.setf(std::ios::right, std::ios::adjustfield);
    s.width((std::streamsize)maxBefore);
    s <<values[j].substr(0,p).c_str();

    if (maxAfter>0){
      s.setf(std::ios::left, std::ios::adjustfield);
      if (p!=std::string::npos){
        s.width((std::streamsize)maxAfter);
        s <<values[j].substr(p,maxAfter).c_str();
      } else{
        assert(maxAfter>1);
        s.width((std::streamsize)maxAfter);
        s <<".0";
      }
    }

    s <<' ';
  }
  s <<std::endl;


  s.flags(original_flags); // restore s to standard state

  return (int)(maxBefore+maxAfter);
}

vpRowVector operator*(const double &x,const vpRowVector &v)
{
  vpRowVector vout ;
  vout = v*x ;
  return vout ;
}

double vpRowVector::sum() const
{
  double sum=0.0;

  for (unsigned int j=0;j<colNum;j++) {
    sum += rowPtrs[0][j];
  }

  return sum;
}

double vpRowVector::sumSquare() const
{
  double sum_square=0.0;

  for (unsigned int j=0;j<colNum;j++) {
    double x=rowPtrs[0][j];
    sum_square += x*x;
  }

  return sum_square;
}

double vpRowVector::euclideanNorm() const
{
  double norm=0.0;
  for (unsigned int i=0;i<dsize;i++) {
    double x = *(data +i); norm += x*x;
  }

  return sqrt(norm);
}

void
vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols)
{
  unsigned int cncols = c+ncols ;

  if (cncols > v.getCols())
    throw(vpException(vpException::dimensionError,
                      "Bad column dimension (%d > %d) used to initialize vpRowVector",
                      cncols, v.getCols()));
  resize(ncols);
  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
    return; // Noting to do
  for (unsigned int i=0 ; i < ncols; i++)
    (*this)[i] = v[i+c];
}

std::ostream & vpRowVector::cppPrint(std::ostream & os, const std::string &matrixName, bool octet) const
{
  os << "vpRowVector " << matrixName
     << " ("<< this ->getCols () << "); " <<std::endl;

  for (unsigned int j=0; j < this ->getCols(); ++ j) {
    if (! octet) {
      os << matrixName << "[" << j
         << "] = " << (*this)[j] << "; " << std::endl;
    }
    else {
      for (unsigned int k = 0; k < sizeof(double); ++ k) {
        os << "((unsigned char*)&(" << matrixName
           << "[" << j << "]) )[" << k
           <<"] = 0x" <<std::hex<<
             (unsigned int)((unsigned char*)& ((*this)[j])) [k]
             << "; " << std::endl;
      }
    }
  }
  std::cout << std::endl;
  return os;
}

std::ostream & vpRowVector::csvPrint(std::ostream & os) const
{
  for (unsigned int j=0; j < this->getCols(); ++ j) {
    os <<  (*this)[j];
    if (!(j==(this->getCols()-1)))
      os << ", ";
  }
  os << std::endl;
  return os;
}

std::ostream & vpRowVector::maplePrint(std::ostream & os) const
{
  os << "([ " << std::endl;
  os << "[";
  for (unsigned int j=0; j < this->getCols(); ++ j) {
    os <<  (*this)[j] << ", ";
  }
  os << "]," << std::endl;
  os << "])" << std::endl;
  return os;
}

std::ostream & vpRowVector::matlabPrint(std::ostream & os) const
{
  os << "[ ";
  for (unsigned int j=0; j < this ->getCols(); ++ j) {
    os <<  (*this)[j] << ", ";
  }
  os << "]" << std::endl;
  return os;
}