YARP
Yet Another Robot Platform
Matrix.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <yarp/sig/Matrix.h>
8
9#include <yarp/conf/system.h>
10
11#include <yarp/sig/Vector.h>
12#include <yarp/os/Bottle.h>
16#include <yarp/os/NetFloat64.h>
17#include <yarp/os/NetInt32.h>
18
19#include <vector>
20#include <cstdio>
21
22using namespace yarp::sig;
23using namespace yarp::os;
24
25#define RES(v) ((std::vector<T> *)v)
26
29{
30public:
39
41};
43
44// network stuff
45#include <yarp/os/NetInt32.h>
46
47bool yarp::sig::removeCols(const Matrix &in, Matrix &out, size_t first_col, size_t how_many)
48{
49 size_t nrows = in.rows();
50 size_t ncols = in.cols();
51 Matrix ret(nrows, ncols-how_many);
52 for (size_t r = 0; r < nrows; r++) {
53 for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
54 {
55 if (c_in==first_col)
56 {
57 c_in=c_in+(how_many-1);
58 continue;
59 }
60 ret[r][c_out]=(in)[r][c_in];
61 c_out++;
62 }
63 }
64 out=ret;
65 return true;
66}
67
68bool yarp::sig::removeRows(const Matrix &in, Matrix &out, size_t first_row, size_t how_many)
69{
70 size_t nrows = in.rows();
71 size_t ncols = in.cols();
72 Matrix ret(nrows-how_many, ncols);
73 for (size_t c = 0; c < ncols; c++) {
74 for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
75 {
76 if (r_in==first_row)
77 {
78 r_in=r_in+(how_many-1);
79 continue;
80 }
81 ret[r_out][c]=(in)[r_in][c];
82 r_out++;
83 }
84 }
85 out=ret;
86 return true;
87}
88
89bool yarp::sig::submatrix(const Matrix &in, Matrix &out, size_t r1, size_t r2, size_t c1, size_t c2)
90{
91 double *t=out.data();
92 const double *i=in.data()+in.cols()*r1+c1;
93 const int offset=in.cols()-(c2-c1+1);
94
95 if (i == nullptr || t == nullptr) {
96 return false;
97 }
98
99 for(size_t r=0;r<=(r2-r1);r++)
100 {
101 for(size_t c=0;c<=(c2-c1);c++)
102 {
103 *t++=*i++;
104 }
105 i+=offset;
106 }
107
108 return true;
109}
110
111
113 // auto-convert text mode interaction
114 connection.convertTextMode();
116 bool ok = connection.expectBlock((char*)&header, sizeof(header));
117 if (!ok) {
118 return false;
119 }
120 size_t r=rows();
121 size_t c=cols();
122 if (header.listLen > 0)
123 {
124 if ( r != (size_t)(header.rows) || c!=(size_t)(header.cols))
125 {
126 resize(header.rows, header.cols);
127 }
128
129 int l=0;
130 double *tmp=data();
131 for (l = 0; l < header.listLen; l++) {
132 tmp[l] = connection.expectFloat64();
133 }
134 } else {
135 return false;
136 }
137
138 return true;
139}
140
141
144
145 //header.totalLen = sizeof(header)+sizeof(double)*this->size();
147 header.outerListLen = 3;
148 header.rowsTag = BOTTLE_TAG_INT32;
149 header.colsTag = BOTTLE_TAG_INT32;
151 header.rows=rows();
152 header.cols=cols();
153 header.listLen = header.rows*header.cols;
154
155 connection.appendBlock((char*)&header, sizeof(header));
156
157 int l=0;
158 const double *tmp=data();
159 for (l = 0; l < header.listLen; l++) {
160 connection.appendFloat64(tmp[l]);
161 }
162
163 // if someone is foolish enough to connect in text mode,
164 // let them see something readable.
165 connection.convertTextMode();
166
167 return true;
168}
169
170
171std::string Matrix::toString(int precision, int width, const char* endRowStr) const {
172
173 // If the width is less than 1, use tabs, else use width number of spaces.
174 std::string spacer((width<0) ? "\t" : " ");
175
176 // Buffering.
177 std::string ret = "";
178 char buffer[350];
179 const double* src = (*this).data();
180
181 // Iterate through copying the contents from the matrix, into a string.
182 // Avoid unnecessary string resizes by only adding spacers at the beginning.
183 size_t r, c;
184 for (r = 0; r < nrows; r++) {
185 if (r) { ret += endRowStr; }
186 for (c = 0; c < ncols; c++) {
187 if (c) { ret += spacer; }
188 sprintf(buffer, "% *.*lf", width, precision, *src); src++;
189 ret += buffer;
190 }
191 }
192
193 return ret;
194}
195
196
197void Matrix::updatePointers()
198{
199 if (matrix != nullptr) {
200 delete[] matrix;
201 }
202
203 size_t r=0;
204 matrix=new double* [nrows];
205 if (nrows > 0) {
206 matrix[0] = storage;
207 }
208 for(r=1;r<nrows; r++)
209 {
210 matrix[r]=matrix[r-1]+ncols;
211 }
212}
213
215{
216 if (this == &r) {
217 return *this;
218 }
219
220 if(nrows!=r.nrows || ncols!=r.ncols)
221 {
222 if (storage) {
223 delete[] storage;
224 }
225
226 nrows=r.nrows;
227 ncols=r.ncols;
228
229 storage=new double[ncols*nrows];
230 memcpy(storage, r.storage, ncols*nrows*sizeof(double));
231 updatePointers();
232 }
233 else
234 {
235 if (!storage) {
236 storage = new double[ncols * nrows];
237 }
238 memcpy(storage, r.storage, ncols*nrows*sizeof(double));
239 }
240
241 return *this;
242}
243
244const Matrix &Matrix::operator=(double v)
245{
246 size_t nelem = nrows*ncols;
247 for (size_t k = 0; k < nelem; k++) {
248 storage[k] = v;
249 }
250
251 return *this;
252}
253
255{
256 if (matrix != nullptr) {
257 delete[] matrix;
258 }
259
260 if (storage != nullptr) {
261 delete[] storage;
262 }
263}
264
265void Matrix::resize(size_t new_r, size_t new_c)
266{
267 if (new_r == nrows && new_c == ncols) {
268 return;
269 }
270
271 auto* new_storage=new double[new_r*new_c];
272
273 const size_t copy_r=(new_r<nrows) ? new_r:nrows;
274 const size_t copy_c=(new_c<ncols) ? new_c:ncols;
275 //copy_r = (new_r<nrows) ? new_r:nrows;
276
277 if (storage!=nullptr)
278 {
279 double *tmp_new=new_storage;
280 double *tmp_current=storage;
281 // copy content
282
283 // favor performance for small matrices
284#if 0
285 const int stepN=(new_c-copy_c);
286 const int stepC=(ncols-copy_c);
287
288 for(int r=0; r<copy_r;r++)
289 {
290 for(int c=0;c<copy_c;c++)
291 *tmp_new++=*tmp_current++;
292 tmp_new+=stepN;
293 tmp_current=matrix[r];
294 }
295#endif
296
297 // favor performance with large matrices
298 for(size_t r=0; r<copy_r;r++)
299 {
300 tmp_current=matrix[r];
301 memcpy(tmp_new, tmp_current, sizeof(double)*copy_c);
302 tmp_new+=new_c;
303 }
304
305
306 delete [] storage;
307 }
308 else
309 {
310 //zero memory
311 memset(new_storage, 0, sizeof(double)*new_r*new_c);
312 }
313
314 storage=new_storage;
315
316 nrows=new_r;
317 ncols=new_c;
318
319 // storage.resize(r*c);
320 updatePointers();
321}
322
324{
325 memset(storage, 0, sizeof(double)*ncols*nrows);
326}
327
328Matrix Matrix::removeCols(size_t first_col, size_t how_many)
329{
330 Matrix ret;
331 ret.resize(nrows, ncols-how_many);
332
333 for (size_t r = 0; r < nrows; r++) {
334 for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
335 {
336 if (c_in==first_col)
337 {
338 c_in=c_in+(how_many-1);
339 continue;
340 }
341 ret[r][c_out]=(*this)[r][c_in];
342 c_out++;
343 }
344 }
345
346 if (storage) {
347 delete[] storage;
348 }
349
350 nrows=ret.nrows;
351 ncols=ret.ncols;
352 storage=new double[ncols*nrows];
353 memcpy(storage, ret.storage, ncols*nrows*sizeof(double));
354 updatePointers();
355 return ret;
356}
357
358Matrix Matrix::removeRows(size_t first_row, size_t how_many)
359{
360 Matrix ret;
361 ret.resize(nrows-how_many, ncols);
362
363 for (size_t c = 0; c < ncols; c++) {
364 for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
365 {
366 if (r_in==first_row)
367 {
368 r_in=r_in+(how_many-1);
369 continue;
370 }
371 ret[r_out][c]=(*this)[r_in][c];
372 r_out++;
373 }
374 }
375
376 if (storage) {
377 delete[] storage;
378 }
379
380 nrows=ret.nrows;
381 ncols=ret.ncols;
382 storage=new double[ncols*nrows];
383 memcpy(storage, ret.storage, ncols*nrows*sizeof(double));
384 updatePointers();
385 return ret;
386}
387
389{
390 Matrix ret;
391 ret.resize(ncols, nrows);
392
393 for (size_t r = 0; r < nrows; r++) {
394 for (size_t c = 0; c < ncols; c++) {
395 ret[c][r] = (*this)[r][c];
396 }
397 }
398
399 return ret;
400}
401
402Vector Matrix::getRow(size_t r) const
403{
404 Vector ret;
405 ret.resize(ncols);
406
407 for (size_t c = 0; c < ncols; c++) {
408 ret[c] = (*this)[r][c];
409 }
410
411 return ret;
412}
413
414Vector Matrix::getCol(size_t c) const
415{
416 Vector ret;
417 ret.resize(nrows);
418
419 for (size_t r = 0; r < nrows; r++) {
420 ret[r] = (*this)[r][c];
421 }
422
423 return ret;
424}
425
426Vector Matrix::subrow(size_t r, size_t c, size_t size) const
427{
428 if (r >= rows() || c + size - 1 >= cols()) {
429 return Vector(0);
430 }
431
432 Vector ret(size);
433
434 for (size_t i = 0; i < size; i++) {
435 ret[i] = (*this)[r][c + i];
436 }
437
438 return ret;
439}
440
441Vector Matrix::subcol(size_t r, size_t c, size_t size) const
442{
443 if (r + size - 1 >= rows() || c >= cols()) {
444 return Vector(0);
445 }
446
447 Vector ret(size);
448
449 for (size_t i = 0; i < size; i++) {
450 ret[i] = (*this)[r + i][c];
451 }
452
453 return ret;
454}
455
457{
458 zero();
459 size_t tmpR=nrows;
460 if (ncols < nrows) {
461 tmpR = ncols;
462 }
463
464 size_t c=0;
465 for (size_t r = 0; r < tmpR; r++, c++) {
466 (*this)[r][c] = 1.0;
467 }
468
469 return *this;
470}
471
473{
474 zero();
475 size_t tmpR=nrows;
476 if (ncols < nrows) {
477 tmpR = ncols;
478 }
479
480 size_t c=0;
481 for (size_t r = 0; r < tmpR; r++, c++) {
482 (*this)[r][c] = d[r];
483 }
484
485 return *this;
486}
487
489{
490 //check dimensions first
491 if ((rows() != r.rows()) || (cols() != r.cols())) {
492 return false;
493 }
494
495 const double *tmp1=data();
496 const double *tmp2=r.data();
497
498 if (tmp1 == nullptr || tmp2 == nullptr) {
499 return false;
500 }
501
502 int c=rows()*cols();
503 while(c--)
504 {
505 if (*tmp1++ != *tmp2++) {
506 return false;
507 }
508 }
509
510 return true;
511}
512
513bool Matrix::setRow(size_t row, const Vector &r)
514{
515 if ((row >= nrows) || (r.length() != ncols)) {
516 return false;
517 }
518
519 for (size_t c = 0; c < ncols; c++) {
520 (*this)[row][c] = r[c];
521 }
522
523 return true;
524}
525
526bool Matrix::setCol(size_t col, const Vector &c)
527{
528 if ((col >= ncols) || (c.length() != nrows)) {
529 return false;
530 }
531
532 for (size_t r = 0; r < nrows; r++) {
533 (*this)[r][col] = c[r];
534 }
535
536 return true;
537}
538
539bool Matrix::setSubmatrix(const yarp::sig::Matrix &m, size_t r, size_t c)
540{
541 if ((c + m.cols() > ncols) || (r + m.rows() > nrows)) {
542 return false;
543 }
544
545 for (size_t i = 0; i < m.rows(); i++) {
546 for (size_t j = 0; j < m.cols(); j++) {
547 (*this)[r + i][c + j] = m(i, j);
548 }
549 }
550 return true;
551}
552
553bool Matrix::setSubrow(const Vector &v, size_t r, size_t c)
554{
555 size_t s = v.size();
556 if (r >= nrows || c + s - 1 >= (size_t)ncols) {
557 return false;
558 }
559
560 for (size_t i = 0; i < s; i++) {
561 (*this)[r][i + c] = v[i];
562 }
563 return true;
564}
565
566bool Matrix::setSubcol(const Vector &v, size_t r, size_t c)
567{
568 size_t s = v.size();
569 if (r + s - 1 >= (size_t)nrows || c >= ncols) {
570 return false;
571 }
572
573 for (size_t i = 0; i < s; i++) {
574 (*this)[r + i][c] = v[i];
575 }
576 return true;
577}
578
579Matrix::Matrix(size_t r, size_t c):
580 storage(nullptr),
581 matrix(nullptr),
582 nrows(r),
583 ncols(c)
584{
585 storage=new double [r*c];
586 memset(storage, 0, r*c*sizeof(double));
587 updatePointers();
588}
589
591 storage(nullptr),
592 matrix(nullptr)
593{
594 nrows=m.nrows;
595 ncols=m.ncols;
596
597 if (m.storage!=nullptr)
598 {
599 storage=new double [nrows*ncols];
600 memcpy(storage, m.storage, nrows*ncols*sizeof(double));
601
602 updatePointers();
603 }
604}
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:25
#define BOTTLE_TAG_INT32
Definition: Bottle.h:21
#define BOTTLE_TAG_LIST
Definition: Bottle.h:28
float t
bool ret
contains the definition of a Matrix type
contains the definition of a Vector type
yarp::os::NetInt32 outerListLen
Definition: Matrix.cpp:32
MatrixPortContentHeader()=default
yarp::os::NetInt32 cols
Definition: Matrix.cpp:36
yarp::os::NetInt32 listLen
Definition: Matrix.cpp:38
yarp::os::NetInt32 rows
Definition: Matrix.cpp:34
yarp::os::NetInt32 listTag
Definition: Matrix.cpp:37
yarp::os::NetInt32 rowsTag
Definition: Matrix.cpp:33
yarp::os::NetInt32 outerListTag
Definition: Matrix.cpp:31
yarp::os::NetInt32 colsTag
Definition: Matrix.cpp:35
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
This is a base class for objects that can be both read from and be written to the YARP network.
Definition: Portable.h:25
A class for a Matrix.
Definition: Matrix.h:39
Matrix removeRows(size_t first_row, size_t how_many)
Modifies the matrix, removing one or more rows from it.
Definition: Matrix.cpp:358
void zero()
Zero the matrix.
Definition: Matrix.cpp:323
Vector getRow(size_t r) const
Get a row of the matrix as a vector.
Definition: Matrix.cpp:402
Vector subcol(size_t r, size_t c, size_t size) const
Get a subcolumn of the matrix as a vector.
Definition: Matrix.cpp:441
void resize(size_t r, size_t c)
Resize the matrix, if matrix is not empty preserve old content.
Definition: Matrix.cpp:265
bool operator==(const yarp::sig::Matrix &r) const
True iff all elements of a match all element of b.
Definition: Matrix.cpp:488
Matrix transposed() const
Return the transposed of the matrix.
Definition: Matrix.cpp:388
bool setCol(size_t col, const Vector &c)
Set a column of the matrix copying the values from a vector: the vector length must be equal to the n...
Definition: Matrix.cpp:526
bool setSubrow(const Vector &v, size_t r, size_t c)
Set a portion of a row of this matrix with the values of the specified vector v.
Definition: Matrix.cpp:553
const Matrix & diagonal(const Vector &d)
Build a diagonal matrix, don't resize.
Definition: Matrix.cpp:472
bool setRow(size_t row, const Vector &r)
Set a row of the matrix copying the values from a vector: the vector length must be equal to the numb...
Definition: Matrix.cpp:513
size_t cols() const
Return number of columns.
Definition: Matrix.h:94
~Matrix()
Destructor.
Definition: Matrix.cpp:254
Vector getCol(size_t c) const
Get a columns of the matrix as a vector.
Definition: Matrix.cpp:414
bool setSubmatrix(const Matrix &m, size_t r, size_t c)
Set a portion of this matrix with the values of the specified matrix m.
Definition: Matrix.cpp:539
double * data()
Return a pointer to the first element.
Definition: Matrix.h:297
bool setSubcol(const Vector &v, size_t r, size_t c)
Set a portion of a column of this matrix with the values of the specified vector v.
Definition: Matrix.cpp:566
bool write(yarp::os::ConnectionWriter &connection) const override
Write vector to a connection.
Definition: Matrix.cpp:142
const Matrix & operator=(const Matrix &r)
Copy operator.
Definition: Matrix.cpp:214
size_t rows() const
Return number of rows.
Definition: Matrix.h:88
Vector subrow(size_t r, size_t c, size_t size) const
Get a subrow of the matrix as a vector.
Definition: Matrix.cpp:426
Matrix removeCols(size_t first_col, size_t how_many)
Modifies the matrix, removing one or more columns from it.
Definition: Matrix.cpp:328
std::string toString(int precision=-1, int width=-1, const char *endRowStr="\n") const
Print matrix to a string.
Definition: Matrix.cpp:171
const Matrix & eye()
Build an identity matrix, don't resize.
Definition: Matrix.cpp:456
size_t size() const
Definition: Vector.h:321
size_t length() const
Get the length of the vector.
Definition: Vector.h:329
An interface to the operating system, including Port based communication.
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:29
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:915
VectorOf< double > Vector
Definition: Vector.h:36
bool removeRows(const Matrix &in, Matrix &out, size_t first_row, size_t how_many)
Definition: Matrix.cpp:68
bool submatrix(const Matrix &in, Matrix &out, size_t r1, size_t r2, size_t c1, size_t c2)
Definition: Matrix.cpp:89
bool removeCols(const Matrix &in, Matrix &out, size_t first_col, size_t how_many)
Definition: Matrix.cpp:47
The main, catch-all namespace for YARP.
Definition: dirs.h:16
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition: system.h:193
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition: system.h:192