vpRowVector.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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 <assert.h>
#include <cmath>
#include <sstream>
#include <stdlib.h>
#include <string.h>

#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRowVector.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];
}

std::vector<double> vpRowVector::toStdVector()
{
  std::vector<double> v(this->size());

  for (unsigned int i = 0; i < this->size(); i++)
    v[i] = data[i];
  return v;
}

void vpRowVector::stack(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 || v.size() == 0) {
    throw(vpException(vpException::dimensionError, "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 || v.size() == 0) {
    throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
  }

  std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);

  return vpMath::getMedian(vectorOfDoubles);
}

double vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
{
  if (v.data == NULL || v.size() == 0) {
    throw(vpException(vpException::dimensionError, "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;
}