MRPT  2.0.5
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-2020, 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 
16 #include <Eigen/Dense>
17 #include <iostream>
18 
19 #if MRPT_HAS_MATLAB
20 #include <mexplus/mxarray.h>
21 #endif
22 
23 using namespace mrpt::obs;
24 using namespace mrpt::math;
25 using namespace mrpt::poses;
26 using namespace mrpt::img;
27 
28 // This must be added to any CSerializable class implementation file.
30 
31 uint8_t CObservationImage::serializeGetVersion() const { return 4; }
33 {
34  out << cameraPose << cameraParams << image << timestamp << sensorLabel;
35 }
36 
38  mrpt::serialization::CArchive& in, uint8_t version)
39 {
40  switch (version)
41  {
42  case 0:
43  case 1:
44  case 2:
45  case 3:
46  case 4:
47  {
48  in >> cameraPose;
49 
50  if (version >= 4)
51  {
52  in >> cameraParams;
53  }
54  else
55  {
56  CMatrixF intrinsicParams, distortionParams;
57  in >> distortionParams >> intrinsicParams;
58 
59  if (distortionParams.rows() == 1 &&
60  distortionParams.cols() == 5)
61  {
63  p = distortionParams.cast_double();
64  cameraParams.setDistortionParamsVector(p);
65  }
66  else
67  cameraParams.dist.fill(0);
68 
69  cameraParams.intrinsicParams = mrpt::math::CMatrixDouble33(
70  intrinsicParams.block<3, 3>(0, 0).cast<double>());
71  }
72 
73  in >> image;
74 
75  if (version >= 1) in >> timestamp;
76 
77  if (version >= 2)
78  {
79  if (version < 4) in >> cameraParams.focalLengthMeters;
80  }
81  else
82  cameraParams.focalLengthMeters = 0.002;
83 
84  if (version >= 3)
85  in >> sensorLabel;
86  else
87  sensorLabel = "";
88  }
89  break;
90  default:
92  };
93 }
94 
95 /*---------------------------------------------------------------
96  Implements the writing to a mxArray for Matlab
97  ---------------------------------------------------------------*/
98 #if MRPT_HAS_MATLAB
99 // Add to implement mexplus::from template specialization
101 #endif
102 
104 {
105 #if MRPT_HAS_MATLAB
106  const char* fields[] = {"class", "ts", "sensorLabel",
107  "image", "pose", "params"};
108  mexplus::MxArray obs_struct(
109  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
110 
111  obs_struct.set("class", this->GetRuntimeClass()->className);
112  obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
113  obs_struct.set("sensorLabel", this->sensorLabel);
114  obs_struct.set("image", this->image);
115  obs_struct.set("pose", this->cameraPose);
116  obs_struct.set("params", this->cameraParams);
117  return obs_struct.release();
118 #else
119  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
120 #endif
121 }
122 
124 {
125  image.undistort(out_img, cameraParams);
126 }
127 
128 void CObservationImage::getDescriptionAsText(std::ostream& o) const
129 {
130  using namespace std;
132 
133  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
134  "base:\n";
135  o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << cameraPose
136  << "\n";
137 
138  o << format(
139  "Focal length: %.03f mm\n", cameraParams.focalLengthMeters * 1000);
140 
141  o << "Intrinsic parameters matrix for the camera:"
142  << "\n"
143  << cameraParams.intrinsicParams.inMatlabFormat() << "\n"
144  << cameraParams.intrinsicParams << "\n";
145 
146  o << "Distorsion parameters for the camera: "
147  << cameraParams.getDistortionParamsAsVector() << "\n";
148 
149  if (image.isExternallyStored())
150  o << " Image is stored externally in file: "
151  << image.getExternalStorageFile() << "\n";
152 
153  if (!image.isEmpty())
154  {
155  o << format(
156  " Image size: %ux%u pixels\n", (unsigned int)image.getWidth(),
157  (unsigned int)image.getHeight());
158 
159  o << " Channels order: " << image.getChannelsOrder() << "\n";
160 
161  o << format(
162  " Rows are stored in top-bottom order: %s\n",
163  image.isOriginTopLeft() ? "YES" : "NO");
164  }
165 }
166 
167 void CObservationImage::load() const { image.forceLoad(); }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:106
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...
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:367
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 load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
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
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
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 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020