MRPT  1.9.9
CObservationImage.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/ops_vectors.h> // << of std::vector()
15 #include <Eigen/Dense>
16 #include <iostream>
17 #if MRPT_HAS_MATLAB
18 #include <mexplus/mxarray.h>
19 #endif
20 
21 using namespace mrpt::obs;
22 using namespace mrpt::math;
23 using namespace mrpt::poses;
24 using namespace mrpt::img;
25 
26 // This must be added to any CSerializable class implementation file.
28 
29 uint8_t CObservationImage::serializeGetVersion() const { return 4; }
31 {
32  out << cameraPose << cameraParams << image << timestamp << sensorLabel;
33 }
34 
36  mrpt::serialization::CArchive& in, uint8_t version)
37 {
38  switch (version)
39  {
40  case 0:
41  case 1:
42  case 2:
43  case 3:
44  case 4:
45  {
46  in >> cameraPose;
47 
48  if (version >= 4)
49  {
50  in >> cameraParams;
51  }
52  else
53  {
54  CMatrixF intrinsicParams, distortionParams;
55  in >> distortionParams >> intrinsicParams;
56 
57  if (distortionParams.rows() == 1 &&
58  distortionParams.cols() == 5)
59  {
61  p = distortionParams.cast_double();
62  cameraParams.setDistortionParamsVector(p);
63  }
64  else
65  cameraParams.dist.fill(0);
66 
67  cameraParams.intrinsicParams = mrpt::math::CMatrixDouble33(
68  intrinsicParams.block<3, 3>(0, 0).cast<double>());
69  }
70 
71  in >> image;
72 
73  if (version >= 1) in >> timestamp;
74 
75  if (version >= 2)
76  {
77  if (version < 4) in >> cameraParams.focalLengthMeters;
78  }
79  else
80  cameraParams.focalLengthMeters = 0.002;
81 
82  if (version >= 3)
83  in >> sensorLabel;
84  else
85  sensorLabel = "";
86  }
87  break;
88  default:
90  };
91 }
92 
93 /*---------------------------------------------------------------
94  Implements the writing to a mxArray for Matlab
95  ---------------------------------------------------------------*/
96 #if MRPT_HAS_MATLAB
97 // Add to implement mexplus::from template specialization
99 #endif
100 
102 {
103 #if MRPT_HAS_MATLAB
104  const char* fields[] = {"class", "ts", "sensorLabel",
105  "image", "pose", "params"};
106  mexplus::MxArray obs_struct(
107  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
108 
109  obs_struct.set("class", this->GetRuntimeClass()->className);
110  obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
111  obs_struct.set("sensorLabel", this->sensorLabel);
112  obs_struct.set("image", this->image);
113  obs_struct.set("pose", this->cameraPose);
114  obs_struct.set("params", this->cameraParams);
115  return obs_struct.release();
116 #else
117  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
118 #endif
119 }
120 
122 {
123  image.undistort(out_img, cameraParams);
124 }
125 
126 void CObservationImage::getDescriptionAsText(std::ostream& o) const
127 {
128  using namespace std;
130 
131  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
132  "base:\n";
133  o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << cameraPose
134  << "\n";
135 
136  o << format(
137  "Focal length: %.03f mm\n", cameraParams.focalLengthMeters * 1000);
138 
139  o << "Intrinsic parameters matrix for the camera:"
140  << "\n"
141  << cameraParams.intrinsicParams.inMatlabFormat() << "\n"
142  << cameraParams.intrinsicParams << "\n";
143 
144  o << "Distorsion parameters for the camera: "
145  << cameraParams.getDistortionParamsAsVector() << "\n";
146 
147  if (image.isExternallyStored())
148  o << " Image is stored externally in file: "
149  << image.getExternalStorageFile() << "\n";
150 
151  if (!image.isEmpty())
152  {
153  o << format(
154  " Image size: %ux%u pixels\n", (unsigned int)image.getWidth(),
155  (unsigned int)image.getHeight());
156 
157  o << " Channels order: " << image.getChannelsOrder() << "\n";
158 
159  o << format(
160  " Rows are stored in top-bottom order: %s\n",
161  image.isOriginTopLeft() ? "YES" : "NO");
162  }
163 }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:58
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void fill(const Scalar &val)
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3555
STL namespace.
CMatrixDynamic< double > cast_double() const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
This base provides a set of functions for maths stuff.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void getUndistortedImage(mrpt::img::CImage &out_img) const
Computes the un-distorted image, using the embeded camera intrinsic & distortion parameters.
This namespace contains representation of robot actions and observations.
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
virtual mxArray * writeToMatlab() const
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.
Definition: CSerializable.h:90
GLuint in
Definition: glext.h:7391
GLfloat GLfloat p
Definition: glext.h:6398
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019