Go to the documentation of this file.
42 #include <mrpt/examples_config.h>
45 std::string
RAWLOG_FILE = MRPT_EXAMPLES_BASE_DIRECTORY
46 "../share/mrpt/datasets/localization_demo.rawlog";
56 "Rawlog file does not exist: %s",
RAWLOG_FILE.c_str());
60 size_t rawlogEntry = 0;
66 rawlog_images_path +=
"/Images";
67 CImage::setImagesPathBase(rawlog_images_path);
81 cout <<
"Reading act/oct pair from rawlog..." << endl;
84 arch, action, observations, rawlogEntry))
102 sImgs ? sImgs->getSensorPose(cameraPose)
103 : Img->getSensorPose(cameraPose);
104 K = sImgs ? sImgs->leftCamera.intrinsicParams
105 : Img->cameraParams.intrinsicParams;
111 if (!laserScan)
continue;
115 laserScan->getSensorPose(laserPose);
117 if (std::abs(laserPose.
yaw()) > 90.0_deg)
124 observations->insertObservationsInto(
128 vector<float> X, Y, Z;
129 vector<float>::iterator itX, itY, itZ;
133 sImgs ? sImgs->imageLeft.getWidth() : Img->image.getWidth();
135 sImgs ? sImgs->imageLeft.getHeight() : Img->image.getHeight();
138 vector<float> imgX, imgY;
139 vector<float>::iterator itImgX, itImgY;
140 imgX.resize(X.size());
141 imgY.resize(Y.size());
144 image = sImgs ? sImgs->imageLeft : Img->image;
149 for (itX = X.begin(), itY = Y.begin(), itZ = Z.begin(),
150 itImgX = imgX.begin(), itImgY = imgY.begin();
151 itX != X.end(); itX++, itY++, itZ++, itImgX++, itImgY++)
155 CPoint3D pCamera(pLaser - cameraPose);
159 *itImgX = -K(0, 0) * ((pCamera.x()) / (pCamera.z())) + K(0, 2);
160 *itImgY = -K(1, 1) * ((pCamera.y()) / (pCamera.z())) + K(1, 2);
162 if (*itImgX > 0 && *itImgX<imgW&& * itImgY> 0 && *itImgY < imgH)
164 *itImgX - 1, *itImgY - 1, *itImgX + 1, *itImgY + 1,
170 observations.reset();
174 std::this_thread::sleep_for(50ms);
197 cerr <<
"EXCEPTION: " << e.what() << endl;
202 cerr <<
"Untyped exception!!";
bool kbhit() noexcept
An OS-independent version of kbhit, which returns true if a key has been pushed.
std::string extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension.
static bool readActionObservationPair(mrpt::serialization::CArchive &inStream, CActionCollection::Ptr &action, CSensoryFrame::Ptr &observations, size_t &rawlogEntry)
Reads a consecutive pair action / observation from the rawlog opened at some input stream.
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
std::shared_ptr< mrpt::obs ::CObservationImage > Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
This namespace contains representation of robot actions and observations.
void showImage(const mrpt::img::CImage &img)
Show a given color or grayscale image on the window.
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
This class creates a window as a graphical user interface (GUI) for displaying images to the user.
Classes for creating GUI windows for 2D and 3D visualization.
A class for storing images as grayscale or RGB bitmaps.
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
double yaw() const
Get the YAW angle (in radians)
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
This base provides a set of functions for maths stuff.
std::shared_ptr< mrpt::obs ::CObservationStereoImages > Ptr
A class used to store a 3D point.
Declares a class derived from "CObservation" that encapsules an image from a camera,...
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
virtual void filledRectangle(int x0, int y0, int x1, int y1, const mrpt::img::TColor color)
Draws a filled rectangle.
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |