Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
vpArray2D.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * This class implements an 2D array as a template class.
32  */
33 #ifndef VP_ARRAY2D_H
34 #define VP_ARRAY2D_H
35 
36 #include <fstream>
37 #include <iostream>
38 #include <limits>
39 #include <math.h>
40 #include <ostream>
41 #include <sstream>
42 #include <stdlib.h>
43 #include <string.h>
44 #include <vector>
45 
46 #include <visp3/core/vpConfig.h>
47 #include <visp3/core/vpException.h>
48 
49 #ifdef VISP_HAVE_NLOHMANN_JSON
50 #include VISP_NLOHMANN_JSON(json.hpp)
51 #endif
52 
53 BEGIN_VISP_NAMESPACE
144 template <class Type> class vpArray2D
145 {
146 public:
148  Type *data;
149 
154  vpArray2D() : data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) { }
155 
160  :
161 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
162  vpArray2D()
163 #else
164  data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0)
165 #endif
166  {
167  resize(A.rowNum, A.colNum, false, false);
168  memcpy(data, A.data, static_cast<size_t>(rowNum) * static_cast<size_t>(colNum) * sizeof(Type));
169  }
170 
177  vpArray2D(unsigned int r, unsigned int c)
178  :
179 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
180  vpArray2D()
181 #else
182  data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0)
183 #endif
184  {
185  resize(r, c);
186  }
187 
195  vpArray2D(unsigned int r, unsigned int c, Type val)
196  :
197 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
198  vpArray2D()
199 #else
200  data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0)
201 #endif
202  {
203  resize(r, c, false, false);
204  *this = val;
205  }
206 
218  vpArray2D(const std::vector<Type> &vec, unsigned int r = 0, unsigned int c = 0)
219  :
220 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
221  vpArray2D()
222 #else
223  data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0)
224 #endif
225  {
226  if ((r > 0) && (c > 0)) {
227  if ((r * c) != vec.size()) {
229  "Cannot initialize vpArray(%d, %d) from std::vector(%d). Wrong dimension", r, c, vec.size()));
230  }
231  resize(r, c, false, false);
232  }
233  else if ((c == 0) && (r == 0)) {
235  "Cannot initialize vpArray(%d, %d) from std::vector(%d). Using rows = 0 and cols = 0 is ambiguous", r, c, vec.size()));
236  }
237  else if (c == 0) {
238  if (r != vec.size()) {
240  "Cannot initialize vpArray(%d, %d) from std::vector(%d). Wrong dimension", r, c, vec.size()));
241  }
242  resize(static_cast<unsigned int>(vec.size()), 1, false, false);
243  }
244  else if (r == 0) {
245  if (c != vec.size()) {
247  "Cannot initialize vpArray(%d, %d) from std::vector(%d). Wrong dimension", r, c, vec.size()));
248  }
249  resize(1, static_cast<unsigned int>(vec.size()), false, false);
250  }
251 
252 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
253  std::copy(vec.begin(), vec.end(), data);
254 #else
255  memcpy(data, vec.data(), vec.size() * sizeof(Type));
256 #endif
257  }
258 
259 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
260  vpArray2D(vpArray2D<Type> &&A) noexcept
261  {
262  rowNum = A.rowNum;
263  colNum = A.colNum;
264  rowPtrs = A.rowPtrs;
265  dsize = A.dsize;
266  data = A.data;
267 
268  A.rowNum = 0;
269  A.colNum = 0;
270  A.rowPtrs = nullptr;
271  A.dsize = 0;
272  A.data = nullptr;
273  }
274 
275  VP_EXPLICIT vpArray2D(const std::initializer_list<Type> &list) : vpArray2D()
276  {
277  resize(1, static_cast<unsigned int>(list.size()), false, false);
278  std::copy(list.begin(), list.end(), data);
279  }
280 
281  VP_EXPLICIT vpArray2D(unsigned int nrows, unsigned int ncols, const std::initializer_list<Type> &list)
282  : data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0)
283  {
284  if ((nrows * ncols) != static_cast<unsigned int>(list.size())) {
285  std::ostringstream oss;
286  oss << "Cannot create a vpArray2D of size (" << nrows << ", " << ncols << ") with a list of size " << list.size();
287  throw vpException(vpException::dimensionError, oss.str());
288  }
289 
290  resize(nrows, ncols, false, false);
291  std::copy(list.begin(), list.end(), data);
292  }
293 
294  VP_EXPLICIT vpArray2D(const std::initializer_list<std::initializer_list<Type> > &lists) : vpArray2D()
295  {
296  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
297  for (auto &l : lists) {
298  if (static_cast<unsigned int>(l.size()) > ncols) {
299  ncols = static_cast<unsigned int>(l.size());
300  }
301  }
302 
303  resize(nrows, ncols, false, false);
304  auto it = lists.begin();
305  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
306  std::copy(it->begin(), it->end(), rowPtrs[i]);
307  }
308  }
309 #endif
310 
314  virtual ~vpArray2D()
315  {
316  if (data != nullptr) {
317  free(data);
318  data = nullptr;
319  }
320 
321  if (rowPtrs != nullptr) {
322  free(rowPtrs);
323  rowPtrs = nullptr;
324  }
325  rowNum = 0;
326  colNum = 0;
327  dsize = 0;
328  }
329 
332 
337  inline unsigned int getCols() const { return colNum; }
338 
339  Type getMaxValue() const;
340 
341  Type getMinValue() const;
342 
347  inline unsigned int getRows() const { return rowNum; }
349  inline unsigned int size() const { return colNum * rowNum; }
350 
362  void resize(unsigned int nrows, unsigned int ncols, bool flagNullify = true, bool recopy_ = true)
363  {
364  if ((nrows == rowNum) && (ncols == colNum)) {
365  if (flagNullify && (this->data != nullptr)) {
366  memset(this->data, 0, this->dsize * sizeof(Type));
367  }
368  }
369  else {
370  bool recopy = (!flagNullify) && recopy_; // priority to flagNullify
371  bool colcond = (ncols != this->colNum) && (this->colNum > 0) && (ncols > 0);
372  const bool recopyNeeded = colcond && ((!flagNullify) || recopy);
373  Type *copyTmp = nullptr;
374  unsigned int rowTmp = 0, colTmp = 0;
375 
376  // Recopy case per case is required if number of cols has changed;
377  // structure of Type array is not the same in this case.
378  if (recopyNeeded && (this->data != nullptr)) {
379  copyTmp = new Type[this->dsize];
380  memcpy(copyTmp, this->data, sizeof(Type) * this->dsize);
381  rowTmp = this->rowNum;
382  colTmp = this->colNum;
383  }
384 
385  // Reallocation of this->data array
386  this->dsize = nrows * ncols;
387  Type *tmp_data = reinterpret_cast<Type *>(realloc(this->data, this->dsize * sizeof(Type)));
388  if (tmp_data) {
389  this->data = tmp_data;
390  }
391  else {
392  this->data = nullptr;
393  }
394 
395  if ((nullptr == this->data) && (0 != this->dsize)) {
396  if (copyTmp != nullptr) {
397  delete[] copyTmp;
398  }
399  throw(vpException(vpException::memoryAllocationError, "Memory allocation error when allocating 2D array data"));
400  }
401 
402  Type **tmp_rowPtrs = reinterpret_cast<Type **>(realloc(this->rowPtrs, nrows * sizeof(Type *)));
403  if (tmp_rowPtrs) {
404  this->rowPtrs = tmp_rowPtrs;
405  }
406  else {
407  this->rowPtrs = nullptr;
408  }
409  if ((nullptr == this->rowPtrs) && (0 != this->dsize)) {
410  if (copyTmp != nullptr) {
411  delete[] copyTmp;
412  }
414  "Memory allocation error when allocating 2D array rowPtrs"));
415  }
416 
417  // Update rowPtrs
418 
419  Type **t_ = rowPtrs;
420  for (unsigned int i = 0; i < dsize; i += ncols) {
421  *t_++ = this->data + i;
422  }
423 
424  this->rowNum = nrows;
425  this->colNum = ncols;
426 
427  // Recopy of this->data array values or nullify
428  if (flagNullify) {
429  // If dsize = 0 nothing to do
430  if ((nullptr != this->data) && (0 != this->dsize)) {
431  memset(this->data, 0, static_cast<size_t>(this->dsize) * sizeof(Type));
432  }
433  }
434  else if (recopyNeeded && (this->rowPtrs != nullptr)) {
435  // Recopy...
436  unsigned int minRow = (this->rowNum < rowTmp) ? this->rowNum : rowTmp;
437  unsigned int minCol = (this->colNum < colTmp) ? this->colNum : colTmp;
438  for (unsigned int i = 0; i < this->rowNum; ++i) {
439  for (unsigned int j = 0; j < this->colNum; ++j) {
440  if ((minRow > i) && (minCol > j)) {
441  (*this)[i][j] = copyTmp[(i * colTmp) + j];
442  }
443  else {
444  (*this)[i][j] = 0;
445  }
446  }
447  }
448  }
449 
450  if (copyTmp != nullptr) {
451  delete[] copyTmp;
452  }
453  }
454  }
455 
456  void reshape(unsigned int nrows, unsigned int ncols)
457  {
458  if (dsize == 0) {
459  resize(nrows, ncols);
460  return;
461  }
462 
463  if ((nrows * ncols) != dsize) {
464  std::ostringstream oss;
465  oss << "Cannot reshape array of total size " << dsize << " into shape (" << nrows << ", " << ncols << ")";
466  throw vpException(vpException::dimensionError, oss.str());
467  }
468 
469  rowNum = nrows;
470  colNum = ncols;
471  if (rowPtrs) {
472  Type **tmp = reinterpret_cast<Type **>(realloc(rowPtrs, nrows * sizeof(Type *)));
473  if (tmp) {
474  this->rowPtrs = tmp;
475  }
476  }
477  if (rowPtrs) {
478  // Update rowPtrs
479  Type **t_ = rowPtrs;
480  for (unsigned int i = 0; i < dsize; i += ncols) {
481  *t_++ = data + i;
482  }
483  }
484  }
485 
486 
497  void insert(const vpArray2D<Type> &A, unsigned int r, unsigned int c)
498  {
499  if (((r + A.getRows()) <= rowNum) && ((c + A.getCols()) <= colNum)) {
500  if ((A.colNum == colNum) && (data != nullptr) && (A.data != nullptr) && (A.data != data)) {
501  memcpy(data + (r * colNum), A.data, sizeof(Type) * A.size());
502  }
503  else if ((data != nullptr) && (A.data != nullptr) && (A.data != data)) {
504  unsigned int a_rows = A.getRows();
505  for (unsigned int i = r; i < (r + a_rows); ++i) {
506  memcpy(data + (i * colNum) + c, A.data + ((i - r) * A.colNum), sizeof(Type) * A.colNum);
507  }
508  }
509  }
510  else {
511  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) array in (%dx%d) array at position (%d,%d)",
512  A.getRows(), A.getCols(), rowNum, colNum, r, c);
513  }
514  }
515 
519  bool operator==(const vpArray2D<Type> &A) const;
523  bool operator!=(const vpArray2D<Type> &A) const;
524 
527  {
528  std::fill(data, data + dsize, x);
529  return *this;
530  }
531 
536  {
537  resize(A.rowNum, A.colNum, false, false);
538  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
539  memcpy(data, A.data, static_cast<size_t>(rowNum) * static_cast<size_t>(colNum) * sizeof(Type));
540  }
541  return *this;
542  }
543 
544 #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher
545  vpArray2D<Type> &operator=(vpArray2D<Type> &&other) noexcept
546  {
547  if (this != &other) {
548  if (data) {
549  free(data);
550  }
551  if (rowPtrs) {
552  free(rowPtrs);
553  }
554 
555  rowNum = other.rowNum;
556  colNum = other.colNum;
557  rowPtrs = other.rowPtrs;
558  dsize = other.dsize;
559  data = other.data;
560 
561  other.rowNum = 0;
562  other.colNum = 0;
563  other.rowPtrs = nullptr;
564  other.dsize = 0;
565  other.data = nullptr;
566  }
567 
568  return *this;
569  }
570 
571  vpArray2D<Type> &operator=(const std::initializer_list<Type> &list)
572  {
573  if (dsize != static_cast<unsigned int>(list.size())) {
574  resize(1, static_cast<unsigned int>(list.size()), false, false);
575  }
576  std::copy(list.begin(), list.end(), data);
577 
578  return *this;
579  }
580 
581  vpArray2D<Type> &operator=(const std::initializer_list<std::initializer_list<Type> > &lists)
582  {
583  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
584  for (auto &l : lists) {
585  if (static_cast<unsigned int>(l.size()) > ncols) {
586  ncols = static_cast<unsigned int>(l.size());
587  }
588  }
589 
590  resize(nrows, ncols, false, false);
591  auto it = lists.begin();
592  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
593  std::copy(it->begin(), it->end(), rowPtrs[i]);
594  }
595 
596  return *this;
597  }
598 
599 #ifdef VISP_HAVE_NLOHMANN_JSON
600  vpArray2D<Type> &operator=(const nlohmann::json &j) = delete;
601 #endif
602 #endif
603 
605  inline Type *operator[](unsigned int i) { return rowPtrs[i]; }
607  inline Type *operator[](unsigned int i) const { return rowPtrs[i]; }
608 
614  friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
615  {
616  if ((A.data == nullptr) || (A.size() == 0)) {
617  return s;
618  }
619  std::ios_base::fmtflags original_flags = s.flags();
620  const unsigned int precision = 10;
621  s.precision(precision);
622  unsigned int a_rows = A.getRows();
623  unsigned int a_cols = A.getCols();
624  for (unsigned int i = 0; i < a_rows; ++i) {
625  for (unsigned int j = 0; j < (a_cols - 1); ++j) {
626  s << A[i][j] << " ";
627  }
628  // We don't add " " after the last row element
629  s << A[i][a_cols - 1];
630  // We don't add a \n char on the end of the last array line
631  if (i < (a_rows - 1)) {
632  s << std::endl;
633  }
634  }
635 
636  s.flags(original_flags); // restore s to standard state
637 
638  return s;
639  }
640 
642 
650 
651  //---------------------------------
652  // Inherited array I/O Static Public Member Functions
653  //---------------------------------
669  static bool load(const std::string &filename, vpArray2D<Type> &A, bool binary = false, char *header = nullptr)
670  {
671  std::fstream file;
672 
673  if (!binary) {
674  file.open(filename.c_str(), std::fstream::in);
675  }
676  else {
677  file.open(filename.c_str(), std::fstream::in | std::fstream::binary);
678  }
679 
680  if (!file) {
681  file.close();
682  return false;
683  }
684 
685  if (!binary) {
686  std::string h;
687  bool headerIsDecoded = false;
688  do {
689  std::streampos pos = file.tellg();
690  char line[FILENAME_MAX];
691  file.getline(line, FILENAME_MAX);
692  std::string prefix("# ");
693  std::string line_(line);
694  if (line_.compare(0, prefix.size(), prefix.c_str()) == 0) {
695  // Line is a comment
696  // If we are not on the first line, we should add "\n" to the end of
697  // the previous line
698  if (pos) {
699  h += "\n";
700  }
701  h += line_.substr(prefix.size()); // Remove "# "
702  }
703  else {
704  // rewind before the line
705  file.seekg(pos, file.beg);
706  headerIsDecoded = true;
707  }
708  } while (!headerIsDecoded);
709 
710  if (header != nullptr) {
711 #if defined(__MINGW32__) || \
712  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
713  snprintf(header, h.size() + 1, "%s", h.c_str());
714 #else
715  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
716 #endif
717  }
718 
719  unsigned int rows, cols;
720  file >> rows;
721  file >> cols;
722 
723  if ((rows >= std::numeric_limits<unsigned int>::max()) || (cols >= std::numeric_limits<unsigned int>::max())) {
724  throw vpException(vpException::badValue, "Array exceed the max size.");
725  }
726 
727  A.resize(rows, cols);
728 
729  Type value;
730  for (unsigned int i = 0; i < rows; ++i) {
731  for (unsigned int j = 0; j < cols; ++j) {
732  file >> value;
733  A[i][j] = value;
734  }
735  }
736  }
737  else {
738  char c = '0';
739  std::string h;
740  // Decode header until '\0' char that ends the header string
741  while (c != '\0') {
742  file.read(&c, 1);
743  h += c;
744  }
745  if (header != nullptr) {
746 #if defined(__MINGW32__) || \
747  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
748  snprintf(header, h.size() + 1, "%s", h.c_str());
749 #else
750  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
751 #endif
752  }
753 
754  unsigned int rows, cols;
755  file.read(reinterpret_cast<char *>(&rows), sizeof(unsigned int));
756  file.read(reinterpret_cast<char *>(&cols), sizeof(unsigned int));
757  A.resize(rows, cols);
758 
759  Type value;
760  for (unsigned int i = 0; i < rows; ++i) {
761  for (unsigned int j = 0; j < cols; ++j) {
762  file.read(reinterpret_cast<char *>(&value), sizeof(Type));
763  A[i][j] = value;
764  }
765  }
766  }
767 
768  file.close();
769  return true;
770  }
771 
783  static bool loadYAML(const std::string &filename, vpArray2D<Type> &A, char *header = nullptr)
784  {
785  std::fstream file;
786 
787  file.open(filename.c_str(), std::fstream::in);
788 
789  if (!file) {
790  file.close();
791  return false;
792  }
793 
794  unsigned int rows = 0, cols = 0;
795  std::string h;
796  std::string line, subs;
797  bool inheader = true;
798  unsigned int i = 0, j;
799  unsigned int lineStart = 0;
800 
801  while (getline(file, line)) {
802  if (inheader) {
803  const std::string str_rows("rows:");
804  const std::string str_cols("cols:");
805  const std::string str_data("data:");
806  if ((rows == 0) && (line.compare(0, str_rows.size(), str_rows.c_str()) == 0)) {
807  std::stringstream ss(line);
808  ss >> subs;
809  ss >> rows;
810  }
811  else if ((cols == 0) && (line.compare(0, str_cols.size(), str_cols.c_str()) == 0)) {
812  std::stringstream ss(line);
813  ss >> subs;
814  ss >> cols;
815  }
816  else if (line.compare(0, str_data.size(), str_data.c_str()) == 0) {
817  inheader = false;
818  }
819  else {
820  h += line + "\n";
821  }
822  }
823  else {
824  // if i == 0, we just got out of the header: initialize matrix
825  // dimensions
826  if (i == 0) {
827  if ((rows == 0) || (cols == 0)) {
828  file.close();
829  return false;
830  }
831  A.resize(rows, cols);
832  // get indentation level which is common to all lines
833  lineStart = static_cast<unsigned int>(line.find("[")) + 1;
834  }
835  std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
836  j = 0;
837  while (getline(ss, subs, ',')) {
838  A[i][j++] = atof(subs.c_str());
839  }
840  ++i;
841  }
842  }
843 
844  if (header != nullptr) {
845  std::string h_ = h.substr(0, h.size() - 1); // Remove last '\n' char
846 #if defined(__MINGW32__) || \
847  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
848  snprintf(header, h_.size() + 1, "%s", h_.c_str());
849 #else
850  _snprintf_s(header, h_.size() + 1, _TRUNCATE, "%s", h_.c_str());
851 #endif
852  }
853 
854  file.close();
855  return true;
856  }
857 
874  static bool save(const std::string &filename, const vpArray2D<Type> &A, bool binary = false, const char *header = "")
875  {
876  std::fstream file;
877 
878  if (!binary) {
879  file.open(filename.c_str(), std::fstream::out);
880  }
881  else {
882  file.open(filename.c_str(), std::fstream::out | std::fstream::binary);
883  }
884 
885  if (!file) {
886  file.close();
887  return false;
888  }
889 
890  if (!binary) {
891  unsigned int i = 0;
892  file << "# ";
893  while (header[i] != '\0') {
894  file << header[i];
895  if (header[i] == '\n') {
896  file << "# ";
897  }
898  ++i;
899  }
900  file << std::endl;
901  file << A.getRows() << "\t" << A.getCols() << std::endl;
902  file << A << std::endl;
903  }
904  else {
905  int headerSize = 0;
906  while (header[headerSize] != '\0') {
907  ++headerSize;
908  }
909  file.write(header, static_cast<size_t>(headerSize)+static_cast<size_t>(1));
910  unsigned int matrixSize;
911  matrixSize = A.getRows();
912  file.write(reinterpret_cast<char *>(&matrixSize), sizeof(unsigned int));
913  matrixSize = A.getCols();
914  file.write(reinterpret_cast<char *>(&matrixSize), sizeof(unsigned int));
915  Type value;
916  unsigned int a_rows = A.getRows();
917  unsigned int a_cols = A.getCols();
918  for (unsigned int i = 0; i < a_rows; ++i) {
919  for (unsigned int j = 0; j < a_cols; ++j) {
920  value = A[i][j];
921  file.write(reinterpret_cast<char *>(&value), sizeof(Type));
922  }
923  }
924  }
925 
926  file.close();
927  return true;
928  }
929 
972  static bool saveYAML(const std::string &filename, const vpArray2D<Type> &A, const char *header = "")
973  {
974  std::fstream file;
975 
976  file.open(filename.c_str(), std::fstream::out);
977 
978  if (!file) {
979  file.close();
980  return false;
981  }
982 
983  unsigned int i = 0;
984  bool inIndent = false;
985  std::string indent = "";
986  bool checkIndent = true;
987  while (header[i] != '\0') {
988  file << header[i];
989  if (checkIndent) {
990  if (inIndent) {
991  if (header[i] == ' ') {
992  indent += " ";
993  }
994  else if (indent.length() > 0) {
995  checkIndent = false;
996  }
997  }
998  if ((header[i] == '\n') || (inIndent && (header[i] == ' '))) {
999  inIndent = true;
1000  }
1001  else {
1002  inIndent = false;
1003  }
1004  }
1005  ++i;
1006  }
1007 
1008  if (i != 0) {
1009  file << std::endl;
1010  }
1011  file << "rows: " << A.getRows() << std::endl;
1012  file << "cols: " << A.getCols() << std::endl;
1013 
1014  if (indent.length() == 0) {
1015  indent = " ";
1016  }
1017 
1018  file << "data: " << std::endl;
1019  unsigned int j;
1020  unsigned int a_rows = A.getRows();
1021  unsigned int a_cols = A.getCols();
1022  for (i = 0; i < a_rows; ++i) {
1023  file << indent << "- [";
1024  for (j = 0; j < (a_cols - 1); ++j) {
1025  file << A[i][j] << ", ";
1026  }
1027  file << A[i][j] << "]" << std::endl;
1028  }
1029 
1030  file.close();
1031  return true;
1032  }
1033 #ifdef VISP_HAVE_NLOHMANN_JSON
1034  //template<typename Type>
1035  template<class T>
1036  friend void from_json(const nlohmann::json &j, vpArray2D<T> &array);
1037  //template<typename Type>
1038  template<class T>
1039  friend void to_json(nlohmann::json &j, const vpArray2D<T> &array);
1040 #endif
1041 
1053  static vpArray2D<Type> conv2(const vpArray2D<Type> &M, const vpArray2D<Type> &kernel, const std::string &mode);
1054 
1067  static void conv2(const vpArray2D<Type> &M, const vpArray2D<Type> &kernel, vpArray2D<Type> &res, const std::string &mode);
1068 
1081  vpArray2D<Type> insert(const vpArray2D<Type> &A, const vpArray2D<Type> &B, unsigned int r, unsigned int c);
1082 
1096  static void insert(const vpArray2D<Type> &A, const vpArray2D<Type> &B, vpArray2D<Type> &C, unsigned int r, unsigned int c);
1098 
1099 protected:
1101  unsigned int rowNum;
1103  unsigned int colNum;
1105  Type **rowPtrs;
1107  unsigned int dsize;
1108 };
1109 
1113 template <class Type> Type vpArray2D<Type>::getMinValue() const
1114 {
1115  Type *dataptr = data;
1116  Type min = *dataptr;
1117  ++dataptr;
1118  for (unsigned int i = 0; i < (dsize - 1); ++i) {
1119  if (*dataptr < min) {
1120  min = *dataptr;
1121  }
1122  ++dataptr;
1123  }
1124  return min;
1125 }
1126 
1130 template <class Type> Type vpArray2D<Type>::getMaxValue() const
1131 {
1132  Type *dataptr = data;
1133  Type max = *dataptr;
1134  ++dataptr;
1135  for (unsigned int i = 0; i < (dsize - 1); ++i) {
1136  if (*dataptr > max) {
1137  max = *dataptr;
1138  }
1139  ++dataptr;
1140  }
1141  return max;
1142 }
1143 
1150 template <class Type> vpArray2D<Type> vpArray2D<Type>::hadamard(const vpArray2D<Type> &m) const
1151 {
1152  if ((m.getRows() != rowNum) || (m.getCols() != colNum)) {
1153  throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
1154  }
1155 
1156  vpArray2D<Type> out;
1157  out.resize(rowNum, colNum, false);
1158 
1159  for (unsigned int i = 0; i < dsize; ++i) {
1160  out.data[i] = data[i] * m.data[i];
1161  }
1162 
1163  return out;
1164 }
1165 
1166 template <class Type> vpArray2D<Type> vpArray2D<Type>::t() const
1167 {
1168  vpArray2D<Type> At(colNum, rowNum);
1169  for (unsigned int i = 0; i < rowNum; ++i) {
1170  for (unsigned int j = 0; j < colNum; ++j) {
1171  At[j][i] = (*this)[i][j];
1172  }
1173  }
1174  return At;
1175 }
1176 
1177 template <class Type> vpArray2D<Type> vpArray2D<Type>::conv2(const vpArray2D<Type> &M, const vpArray2D<Type> &kernel, const std::string &mode)
1178 {
1179  vpArray2D<Type> res;
1180  conv2(M, kernel, res, mode);
1181  return res;
1182 }
1183 
1184 template <class Type> void vpArray2D<Type>::conv2(const vpArray2D<Type> &M, const vpArray2D<Type> &kernel, vpArray2D<Type> &res, const std::string &mode)
1185 {
1186  if (((M.getRows() * M.getCols()) == 0) || ((kernel.getRows() * kernel.getCols()) == 0)) {
1187  return;
1188  }
1189 
1190  if (mode == "valid") {
1191  if ((kernel.getRows() > M.getRows()) || (kernel.getCols() > M.getCols())) {
1192  return;
1193  }
1194  }
1195 
1196  vpArray2D<Type> M_padded, res_same;
1197 
1198  if ((mode == "full") || (mode == "same")) {
1199  const unsigned int pad_x = kernel.getCols() - 1;
1200  const unsigned int pad_y = kernel.getRows() - 1;
1201  const unsigned int pad = 2;
1202  M_padded.resize(M.getRows() + (pad * pad_y), M.getCols() + (pad * pad_x), true, false);
1203  M_padded.insert(M, pad_y, pad_x);
1204 
1205  if (mode == "same") {
1206  res.resize(M.getRows(), M.getCols(), false, false);
1207  res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
1208  }
1209  else {
1210  res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
1211  }
1212  }
1213  else if (mode == "valid") {
1214  M_padded = M;
1215  res.resize((M.getRows() - kernel.getRows()) + 1, (M.getCols() - kernel.getCols()) + 1);
1216  }
1217  else {
1218  return;
1219  }
1220 
1221  if (mode == "same") {
1222  unsigned int res_same_rows = res_same.getRows();
1223  unsigned int res_same_cols = res_same.getCols();
1224  unsigned int kernel_rows = kernel.getRows();
1225  unsigned int kernel_cols = kernel.getCols();
1226  for (unsigned int i = 0; i < res_same_rows; ++i) {
1227  for (unsigned int j = 0; j < res_same_cols; ++j) {
1228  for (unsigned int k = 0; k < kernel_rows; ++k) {
1229  for (unsigned int l = 0; l < kernel_cols; ++l) {
1230  res_same[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
1231  }
1232  }
1233  }
1234  }
1235 
1236  const unsigned int start_i = kernel.getRows() / 2;
1237  const unsigned int start_j = kernel.getCols() / 2;
1238  unsigned int m_rows = M.getRows();
1239  for (unsigned int i = 0; i < m_rows; ++i) {
1240  memcpy(res.data + (i * M.getCols()), res_same.data + ((i + start_i) * res_same.getCols()) + start_j,
1241  sizeof(Type) * M.getCols());
1242  }
1243  }
1244  else {
1245  unsigned int res_rows = res.getRows();
1246  unsigned int res_cols = res.getCols();
1247  unsigned int kernel_rows = kernel.getRows();
1248  unsigned int kernel_cols = kernel.getCols();
1249  for (unsigned int i = 0; i < res_rows; ++i) {
1250  for (unsigned int j = 0; j < res_cols; ++j) {
1251  for (unsigned int k = 0; k < kernel_rows; ++k) {
1252  for (unsigned int l = 0; l < kernel_cols; ++l) {
1253  res[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1];
1254  }
1255  }
1256  }
1257  }
1258  }
1259 }
1260 
1261 template<class Type> vpArray2D<Type> vpArray2D<Type>::insert(const vpArray2D<Type> &A, const vpArray2D<Type> &B, unsigned int r, unsigned int c)
1262 {
1263  vpArray2D<Type> C;
1264 
1265  insert(A, B, C, r, c);
1266 
1267  return C;
1268 }
1269 
1270 template<class Type> void vpArray2D<Type>::insert(const vpArray2D<Type> &A, const vpArray2D<Type> &B, vpArray2D<Type> &C, unsigned int r, unsigned int c)
1271 {
1272  if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) {
1273  C.resize(A.getRows(), A.getCols(), false, false);
1274 
1275  unsigned int a_rows = A.getRows();
1276  unsigned int a_cols = A.getCols();
1277  for (unsigned int i = 0; i < a_rows; ++i) {
1278  for (unsigned int j = 0; j < a_cols; ++j) {
1279  if ((i >= r) && (i < (r + B.getRows())) && (j >= c) && (j < (c + B.getCols()))) {
1280  C[i][j] = B[i - r][j - c];
1281  }
1282  else {
1283  C[i][j] = A[i][j];
1284  }
1285  }
1286  }
1287  }
1288  else {
1289  throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) array in (%dx%d) array at position (%d,%d)",
1290  B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
1291  }
1292 }
1293 
1294 template <class Type> bool vpArray2D<Type>::operator==(const vpArray2D<Type> &A) const
1295 {
1296  if ((A.rowNum != rowNum) || (A.colNum != colNum)) {
1297  return false;
1298  }
1299 
1300  unsigned int a_size = A.size();
1301  for (unsigned int i = 0; i < a_size; ++i) {
1302  if (data[i] != A.data[i]) {
1303  return false;
1304  }
1305  }
1306 
1307  return true;
1308 }
1309 
1313 template <> inline bool vpArray2D<double>::operator==(const vpArray2D<double> &A) const
1314 {
1315  if ((A.rowNum != rowNum) || (A.colNum != colNum)) {
1316  return false;
1317  }
1318 
1319  unsigned int a_size = A.size();
1320  for (unsigned int i = 0; i < a_size; ++i) {
1321  if (fabs(data[i] - A.data[i]) > std::numeric_limits<double>::epsilon()) {
1322  return false;
1323  }
1324  }
1325 
1326  return true;
1327 }
1328 
1332 template <> inline bool vpArray2D<float>::operator==(const vpArray2D<float> &A) const
1333 {
1334  if ((A.rowNum != rowNum) || (A.colNum != colNum)) {
1335  return false;
1336  }
1337 
1338  unsigned int a_size = A.size();
1339  for (unsigned int i = 0; i < a_size; ++i) {
1340  if (fabsf(data[i] - A.data[i]) > std::numeric_limits<float>::epsilon()) {
1341  return false;
1342  }
1343  }
1344 
1345  return true;
1346 }
1347 
1351 template <class Type> bool vpArray2D<Type>::operator!=(const vpArray2D<Type> &A) const { return !(*this == A); }
1352 
1353 #ifdef VISP_HAVE_NLOHMANN_JSON
1354 template <class Type>
1355 inline void from_json(const nlohmann::json &j, vpArray2D<Type> &array)
1356 {
1357  if (j.is_array()) {
1358  const unsigned int nrows = static_cast<unsigned int>(j.size());
1359  if (nrows == 0) { // Initialize an empty array, Finished
1360  array.resize(0, 0);
1361  return;
1362  }
1363  unsigned int ncols = 0;
1364  bool first = true;
1365  for (const auto &item : j) { // Find number of columns, validate that all rows have same number of cols
1366  if (!item.is_array()) {
1367  throw vpException(vpException::badValue, "Trying to instantiate a 2D array with a JSON object that is not an array of array");
1368  }
1369  if (first) {
1370  first = false;
1371  ncols = static_cast<unsigned int>(item.size());
1372  }
1373  else if (ncols != item.size()) {
1374  throw vpException(vpException::badValue, "Trying to instantiate a 2D array with JSON row arrays that are not of the same size");
1375  }
1376  }
1377  array.resize(nrows, ncols);
1378  unsigned i = 0;
1379  for (const auto &item : j) {
1380  std::vector<Type> row = item;
1381  std::copy(row.begin(), row.end(), array.rowPtrs[i]);
1382  ++i;
1383  }
1384  }
1385  else if (j.is_object()) {
1386  const unsigned ncols = j.at("cols");
1387  const unsigned nrows = j.at("rows");
1388  array.resize(nrows, ncols);
1389  const nlohmann::json jData = j.at("data");
1390  if (!jData.is_array() || jData.size() != nrows * ncols) {
1391  std::stringstream ss;
1392  ss << "JSON \"data\" field must be an array of size " << nrows * ncols;
1393  throw vpException(vpException::badValue, ss.str());
1394  }
1395  unsigned i = 0;
1396  for (const auto &jValue : jData) {
1397  array.data[i] = jValue;
1398  ++i;
1399  }
1400  }
1401  else {
1402  throw vpException(vpException::badValue, "Trying to read a vpArray2D from something that is not an array or object");
1403  }
1404 }
1405 
1406 
1407 template <class Type>
1408 inline void to_json(nlohmann::json &j, const vpArray2D<Type> &array)
1409 {
1410  j = {
1411  {"cols", array.colNum},
1412  {"rows", array.rowNum},
1413  {"type", "vpArray2D"}
1414  };
1415 
1416  nlohmann::json::array_t data;
1417  data.reserve(array.size());
1418  for (unsigned i = 0; i < array.size(); ++i) {
1419  data.push_back(array.data[i]);
1420  }
1421  j["data"] = data;
1422 }
1423 #endif
1424 
1425 END_VISP_NAMESPACE
1426 
1427 #endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
vpArray2D< Type > insert(const vpArray2D< Type > &A, const vpArray2D< Type > &B, unsigned int r, unsigned int c)
Definition: vpArray2D.h:1261
static void conv2(const vpArray2D< Type > &M, const vpArray2D< Type > &kernel, vpArray2D< Type > &res, const std::string &mode)
Definition: vpArray2D.h:1184
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
vpArray2D(const vpArray2D< Type > &A)
Definition: vpArray2D.h:159
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Definition: vpArray2D.h:783
Type ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1105
vpArray2D(const std::vector< Type > &vec, unsigned int r=0, unsigned int c=0)
Definition: vpArray2D.h:218
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition: vpArray2D.h:497
vpArray2D< Type > & operator=(const vpArray2D< Type > &A)
Definition: vpArray2D.h:535
Type getMinValue() const
Definition: vpArray2D.h:1113
Type * operator[](unsigned int i)
Set element using A[i][j] = x.
Definition: vpArray2D.h:605
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
static void insert(const vpArray2D< Type > &A, const vpArray2D< Type > &B, vpArray2D< Type > &C, unsigned int r, unsigned int c)
Definition: vpArray2D.h:1270
static bool saveYAML(const std::string &filename, const vpArray2D< Type > &A, const char *header="")
Definition: vpArray2D.h:972
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
friend void to_json(nlohmann::json &j, const vpArray2D< T > &array)
static vpArray2D< Type > conv2(const vpArray2D< Type > &M, const vpArray2D< Type > &kernel, const std::string &mode)
Definition: vpArray2D.h:1177
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:614
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
static bool load(const std::string &filename, vpArray2D< Type > &A, bool binary=false, char *header=nullptr)
Definition: vpArray2D.h:669
vpArray2D< Type > t() const
Compute the transpose of the array.
Definition: vpArray2D.h:1166
unsigned int getRows() const
Definition: vpArray2D.h:347
bool operator!=(const vpArray2D< Type > &A) const
Definition: vpArray2D.h:1351
vpArray2D< Type > & operator=(Type x)
Set all the elements of the array to x.
Definition: vpArray2D.h:526
vpArray2D(unsigned int r, unsigned int c, Type val)
Definition: vpArray2D.h:195
vpArray2D(unsigned int r, unsigned int c)
Definition: vpArray2D.h:177
virtual ~vpArray2D()
Definition: vpArray2D.h:314
vpArray2D< Type > hadamard(const vpArray2D< Type > &m) const
Definition: vpArray2D.h:1150
Type getMaxValue() const
Definition: vpArray2D.h:1130
static bool save(const std::string &filename, const vpArray2D< Type > &A, bool binary=false, const char *header="")
Definition: vpArray2D.h:874
friend void from_json(const nlohmann::json &j, vpArray2D< T > &array)
void reshape(unsigned int nrows, unsigned int ncols)
Definition: vpArray2D.h:456
Type * operator[](unsigned int i) const
Get element using x = A[i][j].
Definition: vpArray2D.h:607
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1103
bool operator==(const vpArray2D< Type > &A) const
Definition: vpArray2D.h:1294
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ memoryAllocationError
Memory allocation error.
Definition: vpException.h:64