Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpArray2D.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * This class implements an 2D array as a template class.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 #ifndef _vpArray2D_h_
39 #define _vpArray2D_h_
40 
41 #include <fstream>
42 #include <iostream>
43 #include <limits>
44 #include <math.h>
45 #include <ostream>
46 #include <sstream>
47 #include <stdlib.h>
48 #include <string.h>
49 
50 #include <visp3/core/vpConfig.h>
51 #include <visp3/core/vpException.h>
52 
70 template <class Type> class vpArray2D
71 {
72 protected:
74  unsigned int rowNum;
76  unsigned int colNum;
78  Type **rowPtrs;
80  unsigned int dsize;
81 
82 public:
84  Type *data;
85 
86 public:
91  vpArray2D<Type>() : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) {}
95  vpArray2D<Type>(const vpArray2D<Type> &A) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
96  {
97  resize(A.rowNum, A.colNum, false, false);
98  memcpy(data, A.data, rowNum * colNum * sizeof(Type));
99  }
106  vpArray2D<Type>(unsigned int r, unsigned int c) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
107  {
108  resize(r, c);
109  }
117  vpArray2D<Type>(unsigned int r, unsigned int c, Type val) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
118  {
119  resize(r, c, false, false);
120  *this = val;
121  }
125  virtual ~vpArray2D<Type>()
126  {
127  if (data != NULL) {
128  free(data);
129  data = NULL;
130  }
131 
132  if (rowPtrs != NULL) {
133  free(rowPtrs);
134  rowPtrs = NULL;
135  }
136  rowNum = colNum = dsize = 0;
137  }
138 
141 
146  inline unsigned int getCols() const { return colNum; }
147 
148  Type getMaxValue() const;
149 
150  Type getMinValue() const;
151 
156  inline unsigned int getRows() const { return rowNum; }
158  inline unsigned int size() const { return colNum * rowNum; }
171  void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify = true,
172  const bool recopy_ = true)
173  {
174  if ((nrows == rowNum) && (ncols == colNum)) {
175  if (flagNullify && this->data != NULL) {
176  memset(this->data, 0, this->dsize * sizeof(Type));
177  }
178  } else {
179  bool recopy = !flagNullify && recopy_; // priority to flagNullify
180  const bool recopyNeeded = (ncols != this->colNum && this->colNum > 0 && ncols > 0 && (!flagNullify || recopy));
181  Type *copyTmp = NULL;
182  unsigned int rowTmp = 0, colTmp = 0;
183 
184  // Recopy case per case is required if number of cols has changed;
185  // structure of Type array is not the same in this case.
186  if (recopyNeeded && this->data != NULL) {
187  copyTmp = new Type[this->dsize];
188  memcpy(copyTmp, this->data, sizeof(Type) * this->dsize);
189  rowTmp = this->rowNum;
190  colTmp = this->colNum;
191  }
192 
193  // Reallocation of this->data array
194  this->dsize = nrows * ncols;
195  this->data = (Type *)realloc(this->data, this->dsize * sizeof(Type));
196  if ((NULL == this->data) && (0 != this->dsize)) {
197  if (copyTmp != NULL) {
198  delete[] copyTmp;
199  }
200  throw(vpException(vpException::memoryAllocationError, "Memory allocation error when allocating 2D array data"));
201  }
202 
203  this->rowPtrs = (Type **)realloc(this->rowPtrs, nrows * sizeof(Type *));
204  if ((NULL == this->rowPtrs) && (0 != this->dsize)) {
205  if (copyTmp != NULL) {
206  delete[] copyTmp;
207  }
209  "Memory allocation error when allocating 2D array rowPtrs"));
210  }
211 
212  // Update rowPtrs
213  {
214  Type **t_ = rowPtrs;
215  for (unsigned int i = 0; i < dsize; i += ncols) {
216  *t_++ = this->data + i;
217  }
218  }
219 
220  this->rowNum = nrows;
221  this->colNum = ncols;
222 
223  // Recopy of this->data array values or nullify
224  if (flagNullify) {
225  memset(this->data, 0, this->dsize * sizeof(Type));
226  } else if (recopyNeeded && this->rowPtrs != NULL) {
227  // Recopy...
228  const unsigned int minRow = (this->rowNum < rowTmp) ? this->rowNum : rowTmp;
229  const unsigned int minCol = (this->colNum < colTmp) ? this->colNum : colTmp;
230  for (unsigned int i = 0; i < this->rowNum; ++i) {
231  for (unsigned int j = 0; j < this->colNum; ++j) {
232  if ((minRow > i) && (minCol > j)) {
233  (*this)[i][j] = copyTmp[i * colTmp + j];
234  } else {
235  (*this)[i][j] = 0;
236  }
237  }
238  }
239  }
240 
241  if (copyTmp != NULL) {
242  delete[] copyTmp;
243  }
244  }
245  }
248  {
249  std::fill(data, data + dsize, x);
250  return *this;
251  }
252 
257  {
258  resize(A.rowNum, A.colNum, false, false);
259  if (data != NULL && A.data != NULL && data != A.data) {
260  memcpy(data, A.data, rowNum * colNum * sizeof(Type));
261  }
262  return *this;
263  }
264 
266  inline Type *operator[](unsigned int i) { return rowPtrs[i]; }
268  inline Type *operator[](unsigned int i) const { return rowPtrs[i]; }
269 
275  friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
276  {
277  if (A.data == NULL || A.size() == 0) {
278  return s;
279  }
280  std::ios_base::fmtflags original_flags = s.flags();
281 
282  s.precision(10);
283  for (unsigned int i = 0; i < A.getRows(); i++) {
284  for (unsigned int j = 0; j < A.getCols() - 1; j++) {
285  s << A[i][j] << " ";
286  }
287  // We don't add " " after the last row element
288  s << A[i][A.getCols() - 1];
289  // We don't add a \n char on the end of the last array line
290  if (i < A.getRows() - 1) {
291  s << std::endl;
292  }
293  }
294 
295  s.flags(original_flags); // restore s to standard state
296 
297  return s;
298  }
299 
300  vpArray2D<Type> hadamard(const vpArray2D<Type> &m) const;
302 
303  //---------------------------------
304  // Inherited array I/O Static Public Member Functions
305  //---------------------------------
322  static bool load(const std::string &filename, vpArray2D<Type> &A, const bool binary = false, char *header = NULL)
323  {
324  std::fstream file;
325 
326  if (!binary) {
327  file.open(filename.c_str(), std::fstream::in);
328  }
329  else {
330  file.open(filename.c_str(), std::fstream::in | std::fstream::binary);
331  }
332 
333  if (!file) {
334  file.close();
335  return false;
336  }
337 
338  if (!binary) {
339  std::string h;
340  bool headerIsDecoded = false;
341  do {
342  std::streampos pos = file.tellg();
343  char line[256];
344  file.getline(line, 256);
345  std::string prefix("# ");
346  std::string line_(line);
347  if (line_.compare(0, 2, prefix.c_str()) == 0) {
348  // Line is a comment
349  // If we are not on the first line, we should add "\n" to the end of
350  // the previous line
351  if (pos) {
352  h += "\n";
353  }
354  h += line_.substr(2); // Remove "# "
355  } else {
356  // rewind before the line
357  file.seekg(pos, file.beg);
358  headerIsDecoded = true;
359  }
360  } while (!headerIsDecoded);
361 
362  if (header != NULL) {
363 #if defined(__MINGW32__) || \
364  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
365  sprintf(header, "%s", h.c_str());
366 #else
367  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
368 #endif
369  }
370 
371  unsigned int rows, cols;
372  file >> rows;
373  file >> cols;
374 
375  if (rows >= (std::numeric_limits<unsigned int>::max)() || cols >= (std::numeric_limits<unsigned int>::max)()) {
376  throw vpException(vpException::badValue, "Array exceed the max size.");
377  }
378 
379  A.resize(rows, cols);
380 
381  Type value;
382  for (unsigned int i = 0; i < rows; i++) {
383  for (unsigned int j = 0; j < cols; j++) {
384  file >> value;
385  A[i][j] = value;
386  }
387  }
388  } else {
389  char c = '0';
390  std::string h;
391  // Decode header until '\0' char that ends the header string
392  while (c != '\0') {
393  file.read(&c, 1);
394  h += c;
395  }
396  if (header != NULL) {
397 #if defined(__MINGW32__) || \
398  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
399  sprintf(header, "%s", h.c_str());
400 #else
401  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
402 #endif
403  }
404 
405  unsigned int rows, cols;
406  file.read((char *)&rows, sizeof(unsigned int));
407  file.read((char *)&cols, sizeof(unsigned int));
408  A.resize(rows, cols);
409 
410  Type value;
411  for (unsigned int i = 0; i < rows; i++) {
412  for (unsigned int j = 0; j < cols; j++) {
413  file.read((char *)&value, sizeof(Type));
414  A[i][j] = value;
415  }
416  }
417  }
418 
419  file.close();
420  return true;
421  }
434  static bool loadYAML(const std::string &filename, vpArray2D<Type> &A, char *header = NULL)
435  {
436  std::fstream file;
437 
438  file.open(filename.c_str(), std::fstream::in);
439 
440  if (!file) {
441  file.close();
442  return false;
443  }
444 
445  unsigned int rows = 0, cols = 0;
446  std::string h;
447  std::string line, subs;
448  bool inheader = true;
449  unsigned int i = 0, j;
450  unsigned int lineStart = 0;
451 
452  while (getline(file, line)) {
453  if (inheader) {
454  if (rows == 0 && line.compare(0, 5, "rows:") == 0) {
455  std::stringstream ss(line);
456  ss >> subs;
457  ss >> rows;
458  } else if (cols == 0 && line.compare(0, 5, "cols:") == 0) {
459  std::stringstream ss(line);
460  ss >> subs;
461  ss >> cols;
462  } else if (line.compare(0, 5, "data:") == 0) {
463  inheader = false;
464  }
465  else {
466  h += line + "\n";
467  }
468  } else {
469  // if i == 0, we just got out of the header: initialize matrix
470  // dimensions
471  if (i == 0) {
472  if (rows == 0 || cols == 0) {
473  file.close();
474  return false;
475  }
476  A.resize(rows, cols);
477  // get indentation level which is common to all lines
478  lineStart = (unsigned int)line.find("[") + 1;
479  }
480  std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
481  j = 0;
482  while (getline(ss, subs, ',')) {
483  A[i][j++] = atof(subs.c_str());
484  }
485  i++;
486  }
487  }
488 
489  if (header != NULL) {
490  std::string h_ = h.substr(0, h.size() - 1); // Remove last '\n' char
491 #if defined(__MINGW32__) || \
492  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
493  sprintf(header, "%s", h_.c_str());
494 #else
495  _snprintf_s(header, h_.size() + 1, _TRUNCATE, "%s", h_.c_str());
496 #endif
497  }
498 
499  file.close();
500  return true;
501  }
502 
519  static bool save(const std::string &filename, const vpArray2D<Type> &A, const bool binary = false,
520  const char *header = "")
521  {
522  std::fstream file;
523 
524  if (!binary) {
525  file.open(filename.c_str(), std::fstream::out);
526  }
527  else {
528  file.open(filename.c_str(), std::fstream::out | std::fstream::binary);
529  }
530 
531  if (!file) {
532  file.close();
533  return false;
534  }
535 
536  if (!binary) {
537  unsigned int i = 0;
538  file << "# ";
539  while (header[i] != '\0') {
540  file << header[i];
541  if (header[i] == '\n') {
542  file << "# ";
543  }
544  i++;
545  }
546  file << std::endl;
547  file << A.getRows() << "\t" << A.getCols() << std::endl;
548  file << A << std::endl;
549  } else {
550  int headerSize = 0;
551  while (header[headerSize] != '\0') {
552  headerSize++;
553  }
554  file.write(header, headerSize + 1);
555  unsigned int matrixSize;
556  matrixSize = A.getRows();
557  file.write((char *)&matrixSize, sizeof(unsigned int));
558  matrixSize = A.getCols();
559  file.write((char *)&matrixSize, sizeof(unsigned int));
560  Type value;
561  for (unsigned int i = 0; i < A.getRows(); i++) {
562  for (unsigned int j = 0; j < A.getCols(); j++) {
563  value = A[i][j];
564  file.write((char *)&value, sizeof(Type));
565  }
566  }
567  }
568 
569  file.close();
570  return true;
571  }
612  static bool saveYAML(const std::string &filename, const vpArray2D<Type> &A, const char *header = "")
613  {
614  std::fstream file;
615 
616  file.open(filename.c_str(), std::fstream::out);
617 
618  if (!file) {
619  file.close();
620  return false;
621  }
622 
623  unsigned int i = 0;
624  bool inIndent = false;
625  std::string indent = "";
626  bool checkIndent = true;
627  while (header[i] != '\0') {
628  file << header[i];
629  if (checkIndent) {
630  if (inIndent) {
631  if (header[i] == ' ') {
632  indent += " ";
633  }
634  else if (indent.length() > 0) {
635  checkIndent = false;
636  }
637  }
638  if (header[i] == '\n' || (inIndent && header[i] == ' ')) {
639  inIndent = true;
640  }
641  else {
642  inIndent = false;
643  }
644  }
645  i++;
646  }
647 
648  if (i != 0) {
649  file << std::endl;
650  }
651  file << "rows: " << A.getRows() << std::endl;
652  file << "cols: " << A.getCols() << std::endl;
653 
654  if (indent.length() == 0) {
655  indent = " ";
656  }
657 
658  file << "data: " << std::endl;
659  unsigned int j;
660  for (i = 0; i < A.getRows(); ++i) {
661  file << indent << "- [";
662  for (j = 0; j < A.getCols() - 1; ++j) {
663  file << A[i][j] << ", ";
664  }
665  file << A[i][j] << "]" << std::endl;
666  }
667 
668  file.close();
669  return true;
670  }
672 };
673 
677 template <class Type> Type vpArray2D<Type>::getMinValue() const
678 {
679  Type *dataptr = data;
680  Type min = *dataptr;
681  dataptr++;
682  for (unsigned int i = 0; i < dsize - 1; i++) {
683  if (*dataptr < min) {
684  min = *dataptr;
685  }
686  dataptr++;
687  }
688  return min;
689 }
690 
694 template <class Type> Type vpArray2D<Type>::getMaxValue() const
695 {
696  Type *dataptr = data;
697  Type max = *dataptr;
698  dataptr++;
699  for (unsigned int i = 0; i < dsize - 1; i++) {
700  if (*dataptr > max) {
701  max = *dataptr;
702  }
703  dataptr++;
704  }
705  return max;
706 }
707 
714 template <class Type> vpArray2D<Type> vpArray2D<Type>::hadamard(const vpArray2D<Type> &m) const
715 {
716  if (m.getRows() != rowNum || m.getCols() != colNum) {
717  throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
718  }
719 
720  vpArray2D<Type> out;
721  out.resize(rowNum, colNum, false);
722 
723  for (unsigned int i = 0; i < dsize; i++) {
724  out.data[i] = data[i] * m.data[i];
725  }
726 
727  return out;
728 }
729 
730 #endif
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
static bool save(const std::string &filename, const vpArray2D< Type > &A, const bool binary=false, const char *header="")
Definition: vpArray2D.h:519
static bool saveYAML(const std::string &filename, const vpArray2D< Type > &A, const char *header="")
Definition: vpArray2D.h:612
vpArray2D< Type > & operator=(Type x)
Set all the elements of the array to x.
Definition: vpArray2D.h:247
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:434
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
error that can be emited by ViSP classes.
Definition: vpException.h:71
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
Implementation of a generic 2D array used as vase class of matrices and vectors.
Definition: vpArray2D.h:70
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
unsigned int getCols() const
Definition: vpArray2D.h:146
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:74
vpArray2D< Type > & operator=(const vpArray2D< Type > &A)
Definition: vpArray2D.h:256
static bool load(const std::string &filename, vpArray2D< Type > &A, const bool binary=false, char *header=NULL)
Definition: vpArray2D.h:322
Memory allocation error.
Definition: vpException.h:88
unsigned int getRows() const
Definition: vpArray2D.h:156
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:76
vpArray2D< Type > hadamard(const vpArray2D< Type > &m) const
Definition: vpArray2D.h:714
Type * operator[](unsigned int i)
Set element using A[i][j] = x.
Definition: vpArray2D.h:266
Type * operator[](unsigned int i) const
Get element using x = A[i][j].
Definition: vpArray2D.h:268
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:80
Type getMaxValue() const
Definition: vpArray2D.h:694
Type getMinValue() const
Definition: vpArray2D.h:677
Type ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:78