vpRowVector.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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 https://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.
 */
 
#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/vpException.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRowVector.h>
 
#if defined(VISP_HAVE_SIMDLIB)
#include <Simd/SimdLib.h>
#endif
 
BEGIN_VISP_NAMESPACE
 
vpRowVector vpRowVector::view(double *data, unsigned int cols)
{
  vpRowVector v;
  vpArray2D<double>::view(v, data, 1, cols);
  return v;
}
 
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)
{
  unsigned int v_size = static_cast<unsigned int>(v.size());
  resize(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)
{
  unsigned int v_size = static_cast<unsigned int>(v.size());
  resize(v_size);
  for (unsigned int i = 0; i < v_size; ++i) {
    (*this)[i] = static_cast<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;
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vpRowVector &vpRowVector::operator=(vpRowVector &&other)
{
  vpArray2D<double>::operator=(std::move(other));
  return *this;
}
 
vpRowVector &vpRowVector::operator=(const std::initializer_list<double> &list)
{
  resize(1, static_cast<unsigned int>(list.size()), false);
  std::copy(list.begin(), list.end(), data);
  return *this;
}
#endif
 
bool vpRowVector::operator==(const vpRowVector &v) const
{
  if ((colNum != v.colNum) || (rowNum != v.rowNum) /* should not happen */) {
    return false;
  }
 
  for (unsigned int i = 0; i < colNum; ++i) {
    if (!vpMath::equal(data[i], v.data[i], std::numeric_limits<double>::epsilon())) {
      return false;
    }
  }
 
  return true;
}
 
bool vpRowVector::operator!=(const vpRowVector &v) const { return !(*this == v); }
 
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
    unsigned int m_cols = M.getCols();
    for (unsigned int j = 0; j < m_cols; ++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;
    ++d;
  }
  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;
    ++d;
  }
  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);
    ++d;
  }
 
  return A;
}
 
vpRowVector vpRowVector::operator-(const vpRowVector &m) const
{
  if (getCols() != m.getCols()) {
    throw(vpException(vpException::dimensionError, "Cannot subtract (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 subtract (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;
}
 
vpRowVector &vpRowVector::operator<<(double val)
{
  resize(1, false);
  data[0] = val;
  return *this;
}
 
vpRowVector &vpRowVector::operator,(double val)
{
  resize(colNum + 1, false);
  data[colNum - 1] = val;
  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())
{
  unsigned int m_cols = M.getCols();
  for (unsigned int j = 0; j < m_cols; ++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()));
  }
  unsigned int m_cols = M.getCols();
  for (unsigned int j = 0; j < m_cols; ++j) {
    (*this)[j] = M[0][j];
  }
}
 
vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, static_cast<unsigned int>(v.size()))
{
  unsigned int v_size = static_cast<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, static_cast<unsigned int>(v.size()))
{
  unsigned int v_size = static_cast<unsigned int>(v.size());
  for (unsigned int j = 0; j < v_size; ++j) {
    (*this)[j] = static_cast<double>(v[j]);
  }
}
 
vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
{
  init(v, c, ncols);
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vpRowVector::vpRowVector(vpRowVector &&v) : vpArray2D<double>(std::move(v))
{ }
#endif
 
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(unsigned int nrows, 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));
  }
  unsigned int v_size = v.size();
  for (unsigned int j = 0; j < v_size; ++j) {
    (*this)[i + j] = v[j];
  }
}
 
std::vector<double> vpRowVector::toStdVector() const
{
  std::vector<double> v(this->size());
 
  unsigned int this_size = 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 == nullptr) || (v.size() == 0)) {
    throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
  }
 
  double mean = 0;
  double *vd = v.data;
  unsigned int v_col = v.getCols();
  for (unsigned int i = 0; i < v_col; ++i) {
    mean += *(vd++);
  }
 
  return mean / v.getCols();
}
 
double vpRowVector::median(const vpRowVector &v)
{
  if ((v.data == nullptr) || (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, bool useBesselCorrection)
{
  if ((v.data == nullptr) || (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;
  unsigned int v_size = v.size();
  for (unsigned int i = 0; i < v_size; ++i) {
    sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
  }
 
  double divisor = static_cast<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<size_type>(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(static_cast<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(static_cast<std::streamsize>(maxAfter));
        s << values[j].substr(p, maxAfter).c_str();
      }
      else {
        assert(maxAfter > 1);
        s.width(static_cast<std::streamsize> (maxAfter));
        s << ".0";
      }
    }
 
    s << ' ';
  }
  s << std::endl;
 
  s.flags(original_flags); // restore s to standard state
 
  return static_cast<int>(maxBefore + maxAfter);
}
 
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::frobeniusNorm() const
{
  double norm = sumSquare();
 
  return sqrt(norm);
}
 
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
double vpRowVector::euclideanNorm() const { return frobeniusNorm(); }
#endif
 
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 == nullptr) { // 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;
 
  unsigned int this_cols = this->getCols();
  for (unsigned int j = 0; j < this_cols; ++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
          << static_cast<unsigned int>(((unsigned char *)&((*this)[j]))[k]) << "; " << std::endl;
      }
    }
  }
  std::cout << std::endl;
  return os;
}
 
std::ostream &vpRowVector::csvPrint(std::ostream &os) const
{
  unsigned int this_cols = this->getCols();
  for (unsigned int j = 0; j < this_cols; ++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 << "[";
  unsigned int this_cols = this->getCols();
  for (unsigned int j = 0; j < this_cols; ++j) {
    os << (*this)[j] << ", ";
  }
  os << "]," << std::endl;
  os << "])" << std::endl;
  return os;
}
 
std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
{
  os << "[ ";
 
  unsigned int this_cols = this->getCols();
  for (unsigned int j = 0; j < this_cols; ++j) {
    os << (*this)[j] << ", ";
  }
  os << "]" << std::endl;
  return os;
}
 
vpRowVector operator*(const double &x, const vpRowVector &v)
{
  vpRowVector vout;
  vout = v * x;
  return vout;
}
 
vpRowVector vpRowVector::hadamard(const vpRowVector &v) const
{
  if ((v.getRows() != rowNum) || (v.getCols() != colNum)) {
    throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
  }
 
  vpRowVector out;
  out.resize(colNum, false);
#if defined(VISP_HAVE_SIMDLIB)
  SimdVectorHadamard(data, v.data, colNum, out.data);
#else
  for (unsigned int i = 0; i < dsize; ++i) {
    out.data[i] = data[i] * v.data[i];
  }
#endif
  return out;
}
END_VISP_NAMESPACE