Visual Servoing Platform  version 3.1.0
vpArray2D.h
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  throw(vpException(vpException::memoryAllocationError, "Memory allocation error when allocating 2D array data"));
200  }
201 
202  this->rowPtrs = (Type **)realloc(this->rowPtrs, nrows * sizeof(Type *));
203  if ((NULL == this->rowPtrs) && (0 != this->dsize)) {
204  if (copyTmp != NULL)
205  delete[] copyTmp;
207  "Memory allocation error when allocating 2D array rowPtrs"));
208  }
209 
210  // Update rowPtrs
211  {
212  Type **t_ = rowPtrs;
213  for (unsigned int i = 0; i < dsize; i += ncols) {
214  *t_++ = this->data + i;
215  }
216  }
217 
218  this->rowNum = nrows;
219  this->colNum = ncols;
220 
221  // Recopy of this->data array values or nullify
222  if (flagNullify) {
223  memset(this->data, 0, this->dsize * sizeof(Type));
224  } else if (recopyNeeded && this->rowPtrs != NULL) {
225  // Recopy...
226  const unsigned int minRow = (this->rowNum < rowTmp) ? this->rowNum : rowTmp;
227  const unsigned int minCol = (this->colNum < colTmp) ? this->colNum : colTmp;
228  for (unsigned int i = 0; i < this->rowNum; ++i) {
229  for (unsigned int j = 0; j < this->colNum; ++j) {
230  if ((minRow > i) && (minCol > j)) {
231  (*this)[i][j] = copyTmp[i * colTmp + j];
232  } else {
233  (*this)[i][j] = 0;
234  }
235  }
236  }
237  }
238 
239  if (copyTmp != NULL)
240  delete[] copyTmp;
241  }
242  }
245  {
246  for (unsigned int i = 0; i < rowNum; i++)
247  for (unsigned int j = 0; j < colNum; j++)
248  rowPtrs[i][j] = x;
249 
250  return *this;
251  }
252 
257  {
258  resize(A.rowNum, A.colNum, false, false);
259  memcpy(data, A.data, rowNum * colNum * sizeof(Type));
260  return *this;
261  }
262 
264  inline Type *operator[](unsigned int i) { return rowPtrs[i]; }
266  inline Type *operator[](unsigned int i) const { return rowPtrs[i]; }
267 
273  friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
274  {
275  if (A.data == NULL || A.size() == 0)
276  return s;
277  std::ios_base::fmtflags original_flags = s.flags();
278 
279  s.precision(10);
280  for (unsigned int i = 0; i < A.getRows(); i++) {
281  for (unsigned int j = 0; j < A.getCols() - 1; j++) {
282  s << A[i][j] << " ";
283  }
284  // We don't add " " after the last row element
285  s << A[i][A.getCols() - 1];
286  // We don't add a \n char on the end of the last array line
287  if (i < A.getRows() - 1)
288  s << std::endl;
289  }
290 
291  s.flags(original_flags); // restore s to standard state
292 
293  return s;
294  }
295 
296  vpArray2D<Type> hadamard(const vpArray2D<Type> &m) const;
298 
299  //---------------------------------
300  // Inherited array I/O Static Public Member Functions
301  //---------------------------------
318  static bool load(const std::string &filename, vpArray2D<Type> &A, const bool binary = false, char *header = NULL)
319  {
320  std::fstream file;
321 
322  if (!binary)
323  file.open(filename.c_str(), std::fstream::in);
324  else
325  file.open(filename.c_str(), std::fstream::in | std::fstream::binary);
326 
327  if (!file) {
328  file.close();
329  return false;
330  }
331 
332  if (!binary) {
333  std::string h;
334  bool headerIsDecoded = false;
335  do {
336  std::streampos pos = file.tellg();
337  char line[256];
338  file.getline(line, 256);
339  std::string prefix("# ");
340  std::string line_(line);
341  if (line_.compare(0, 2, prefix.c_str()) == 0) {
342  // Line is a comment
343  // If we are not on the first line, we should add "\n" to the end of
344  // the previous line
345  if (pos)
346  h += "\n";
347  h += line_.substr(2); // Remove "# "
348  } else {
349  // rewind before the line
350  file.seekg(pos, file.beg);
351  headerIsDecoded = true;
352  }
353  } while (!headerIsDecoded);
354 
355  if (header != NULL) {
356 #if defined(__MINGW32__) || \
357  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
358  sprintf(header, "%s", h.c_str());
359 #else
360  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
361 #endif
362  }
363 
364  unsigned int rows, cols;
365  file >> rows;
366  file >> cols;
367 
368  if (rows >= (std::numeric_limits<unsigned int>::max)() || cols >= (std::numeric_limits<unsigned int>::max)())
369  throw vpException(vpException::badValue, "Array exceed the max size.");
370 
371  A.resize(rows, cols);
372 
373  Type value;
374  for (unsigned int i = 0; i < rows; i++) {
375  for (unsigned int j = 0; j < cols; j++) {
376  file >> value;
377  A[i][j] = value;
378  }
379  }
380  } else {
381  char c = '0';
382  std::string h;
383  // Decode header until '\0' char that ends the header string
384  while ((c != '\0')) {
385  file.read(&c, 1);
386  h += c;
387  }
388  if (header != NULL) {
389 #if defined(__MINGW32__) || \
390  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
391  sprintf(header, "%s", h.c_str());
392 #else
393  _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
394 #endif
395  }
396 
397  unsigned int rows, cols;
398  file.read((char *)&rows, sizeof(unsigned int));
399  file.read((char *)&cols, sizeof(unsigned int));
400  A.resize(rows, cols);
401 
402  Type value;
403  for (unsigned int i = 0; i < rows; i++) {
404  for (unsigned int j = 0; j < cols; j++) {
405  file.read((char *)&value, sizeof(Type));
406  A[i][j] = value;
407  }
408  }
409  }
410 
411  file.close();
412  return true;
413  }
426  static bool loadYAML(const std::string &filename, vpArray2D<Type> &A, char *header = NULL)
427  {
428  std::fstream file;
429 
430  file.open(filename.c_str(), std::fstream::in);
431 
432  if (!file) {
433  file.close();
434  return false;
435  }
436 
437  unsigned int rows = 0, cols = 0;
438  std::string h;
439  std::string line, subs;
440  bool inheader = true;
441  unsigned int i = 0, j;
442  unsigned int lineStart = 0;
443 
444  while (getline(file, line)) {
445  if (inheader) {
446  if (rows == 0 && line.compare(0, 5, "rows:") == 0) {
447  std::stringstream ss(line);
448  ss >> subs;
449  ss >> rows;
450  } else if (cols == 0 && line.compare(0, 5, "cols:") == 0) {
451  std::stringstream ss(line);
452  ss >> subs;
453  ss >> cols;
454  } else if (line.compare(0, 5, "data:") == 0)
455  inheader = false;
456  else
457  h += line + "\n";
458  } else {
459  // if i == 0, we just got out of the header: initialize matrix
460  // dimensions
461  if (i == 0) {
462  if (rows == 0 || cols == 0) {
463  file.close();
464  return false;
465  }
466  A.resize(rows, cols);
467  // get indentation level which is common to all lines
468  lineStart = (unsigned int)line.find("[") + 1;
469  }
470  std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
471  j = 0;
472  while (getline(ss, subs, ','))
473  A[i][j++] = atof(subs.c_str());
474  i++;
475  }
476  }
477 
478  if (header != NULL) {
479  std::string h_ = h.substr(0, h.size() - 1); // Remove last '\n' char
480 #if defined(__MINGW32__) || \
481  !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
482  sprintf(header, "%s", h_.c_str());
483 #else
484  _snprintf_s(header, h_.size() + 1, _TRUNCATE, "%s", h_.c_str());
485 #endif
486  }
487 
488  file.close();
489  return true;
490  }
491 
508  static bool save(const std::string &filename, const vpArray2D<Type> &A, const bool binary = false,
509  const char *header = "")
510  {
511  std::fstream file;
512 
513  if (!binary)
514  file.open(filename.c_str(), std::fstream::out);
515  else
516  file.open(filename.c_str(), std::fstream::out | std::fstream::binary);
517 
518  if (!file) {
519  file.close();
520  return false;
521  }
522 
523  if (!binary) {
524  unsigned int i = 0;
525  file << "# ";
526  while (header[i] != '\0') {
527  file << header[i];
528  if (header[i] == '\n')
529  file << "# ";
530  i++;
531  }
532  file << std::endl;
533  file << A.getRows() << "\t" << A.getCols() << std::endl;
534  file << A << std::endl;
535  } else {
536  int headerSize = 0;
537  while (header[headerSize] != '\0')
538  headerSize++;
539  file.write(header, headerSize + 1);
540  unsigned int matrixSize;
541  matrixSize = A.getRows();
542  file.write((char *)&matrixSize, sizeof(unsigned int));
543  matrixSize = A.getCols();
544  file.write((char *)&matrixSize, sizeof(unsigned int));
545  Type value;
546  for (unsigned int i = 0; i < A.getRows(); i++) {
547  for (unsigned int j = 0; j < A.getCols(); j++) {
548  value = A[i][j];
549  file.write((char *)&value, sizeof(Type));
550  }
551  }
552  }
553 
554  file.close();
555  return true;
556  }
597  static bool saveYAML(const std::string &filename, const vpArray2D<Type> &A, const char *header = "")
598  {
599  std::fstream file;
600 
601  file.open(filename.c_str(), std::fstream::out);
602 
603  if (!file) {
604  file.close();
605  return false;
606  }
607 
608  unsigned int i = 0;
609  bool inIndent = false;
610  std::string indent = "";
611  bool checkIndent = true;
612  while (header[i] != '\0') {
613  file << header[i];
614  if (checkIndent) {
615  if (inIndent) {
616  if (header[i] == ' ')
617  indent += " ";
618  else if (indent.length() > 0)
619  checkIndent = false;
620  }
621  if (header[i] == '\n' || (inIndent && header[i] == ' '))
622  inIndent = true;
623  else
624  inIndent = false;
625  }
626  i++;
627  }
628 
629  if (i != 0)
630  file << std::endl;
631  file << "rows: " << A.getRows() << std::endl;
632  file << "cols: " << A.getCols() << std::endl;
633 
634  if (indent.length() == 0)
635  indent = " ";
636 
637  file << "data: " << std::endl;
638  unsigned int j;
639  for (i = 0; i < A.getRows(); ++i) {
640  file << indent << "- [";
641  for (j = 0; j < A.getCols() - 1; ++j)
642  file << A[i][j] << ", ";
643  file << A[i][j] << "]" << std::endl;
644  }
645 
646  file.close();
647  return true;
648  }
650 };
651 
655 template <class Type> Type vpArray2D<Type>::getMinValue() const
656 {
657  Type *dataptr = data;
658  Type min = *dataptr;
659  dataptr++;
660  for (unsigned int i = 0; i < dsize - 1; i++) {
661  if (*dataptr < min)
662  min = *dataptr;
663  dataptr++;
664  }
665  return min;
666 }
667 
671 template <class Type> Type vpArray2D<Type>::getMaxValue() const
672 {
673  Type *dataptr = data;
674  Type max = *dataptr;
675  dataptr++;
676  for (unsigned int i = 0; i < dsize - 1; i++) {
677  if (*dataptr > max)
678  max = *dataptr;
679  dataptr++;
680  }
681  return max;
682 }
683 
690 template <class Type> vpArray2D<Type> vpArray2D<Type>::hadamard(const vpArray2D<Type> &m) const
691 {
692  if (m.getRows() != rowNum || m.getCols() != colNum) {
693  throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
694  }
695 
696  vpArray2D<Type> out;
697  out.resize(rowNum, colNum, false);
698 
699  for (unsigned int i = 0; i < dsize; i++) {
700  out.data[i] = data[i] * m.data[i];
701  }
702 
703  return out;
704 }
705 
706 #endif
static bool save(const std::string &filename, const vpArray2D< Type > &A, const bool binary=false, const char *header="")
Definition: vpArray2D.h:508
static bool saveYAML(const std::string &filename, const vpArray2D< Type > &A, const char *header="")
Definition: vpArray2D.h:597
vpArray2D< Type > & operator=(Type x)
Set all the elements of the array to x.
Definition: vpArray2D.h:244
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:426
Type * operator[](unsigned int i) const
Get element using x = A[i][j].
Definition: vpArray2D.h:266
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
unsigned int getRows() const
Definition: vpArray2D.h:156
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
Type getMaxValue() const
Definition: vpArray2D.h:671
unsigned int getCols() const
Definition: vpArray2D.h:146
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:74
Type getMinValue() const
Definition: vpArray2D.h:655
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:318
vpArray2D< Type > hadamard(const vpArray2D< Type > &m) const
Definition: vpArray2D.h:690
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:76
Type * operator[](unsigned int i)
Set element using A[i][j] = x.
Definition: vpArray2D.h:264
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:80
Type ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:78