Go to the documentation of this file.
18 #include <mrpt/3rdparty/do_opencv_includes.h>
33 #if MRPT_HAS_KINECT_FREENECT
34 #if defined(__APPLE__)
35 #include <libfreenect/libfreenect.h>
37 #include <libfreenect.h>
44 #if MRPT_HAS_KINECT_FREENECT
46 #define f_ctx reinterpret_cast<freenect_context*>(m_f_ctx)
47 #define f_ctx_ptr reinterpret_cast<freenect_context**>(&m_f_ctx)
48 #define f_dev reinterpret_cast<freenect_device*>(m_f_dev)
49 #define f_dev_ptr reinterpret_cast<freenect_device**>(&m_f_dev)
50 #endif // MRPT_HAS_KINECT_FREENECT
52 #ifdef KINECT_PROFILE_MEM_ALLOC
59 #ifdef MRPT_KINECT_DEPTH_10BIT
60 const float k1 = 1.1863f;
61 const float k2 = 2842.5f;
62 const float k3 = 0.1236f;
66 static_cast<uint16_t
>(k3 * tanf(i / k2 + k1) / 1e-3f);
71 static_cast<uint16_t
>(1e+3f / (i * (-0.0030711016) + 3.3309495161));
75 m_range2meters[0] = 0;
83 : m_sensorPoseOnRobot(),
84 m_relativePoseIntensityWRTDepth(
85 0, -0.02, 0, -90.0_deg, 0.0_deg, -90.0_deg)
122 "MRPT was compiled without support for Kinect. Please, rebuild it.");
142 bool thereIs, hwError;
145 std::make_shared<CObservation3DRangeScan>();
160 vector<CSerializable::Ptr> objs;
162 objs.push_back(newObs);
177 const std::string& iniSection)
180 configSource.
read_float(iniSection,
"pose_x", 0),
181 configSource.
read_float(iniSection,
"pose_y", 0),
182 configSource.
read_float(iniSection,
"pose_z", 0),
213 catch (
const std::exception& e)
215 std::cout <<
"[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
216 "error loading calibration parameters:\n"
241 iniSection,
"relativePoseIntensityWRTDepth",
"");
251 #if MRPT_HAS_KINECT_FREENECT
252 return f_dev !=
nullptr;
260 #if MRPT_HAS_KINECT_FREENECT
262 void depth_cb(freenect_device* dev,
void* v_depth, uint32_t timestamp)
264 const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
266 auto* depth =
reinterpret_cast<uint16_t*
>(v_depth);
268 auto* obj =
reinterpret_cast<CKinect*
>(freenect_get_user(dev));
271 std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
277 #ifdef KINECT_PROFILE_MEM_ALLOC
278 alloc_tim.
enter(
"depth_cb alloc");
284 #ifdef KINECT_PROFILE_MEM_ALLOC
285 alloc_tim.
leave(
"depth_cb alloc");
291 for (
int r = 0; r < frMode.height; r++)
292 for (
int c = 0; c < frMode.width; c++)
297 const uint16_t v = *depth++;
300 obj->internal_tim_latest_depth() = timestamp;
303 void rgb_cb(freenect_device* dev,
void* img_data, uint32_t timestamp)
305 auto* obj =
reinterpret_cast<CKinect*
>(freenect_get_user(dev));
306 const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
309 std::lock_guard<std::mutex> lock(obj->internal_latest_obs_cs());
312 #ifdef KINECT_PROFILE_MEM_ALLOC
313 alloc_tim.
enter(
"depth_rgb loadFromMemoryBuffer");
330 const cv::Mat src_img_bayer(
331 frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
336 cv::cvtColor(src_img_bayer, dst_img_RGB, cv::COLOR_BayerGB2BGR);
346 frMode.width, frMode.height,
348 reinterpret_cast<unsigned char*
>(img_data));
353 #ifdef KINECT_PROFILE_MEM_ALLOC
354 alloc_tim.
leave(
"depth_rgb loadFromMemoryBuffer");
357 obj->internal_tim_latest_rgb() = timestamp;
360 #endif // MRPT_HAS_KINECT_FREENECT
370 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect
372 if (freenect_init(f_ctx_ptr,
nullptr) < 0)
375 freenect_set_log_level(
384 int nr_devices = freenect_num_devices(f_ctx);
397 freenect_set_led(f_dev, LED_RED);
398 freenect_set_depth_callback(f_dev, depth_cb);
399 freenect_set_video_callback(f_dev, rgb_cb);
402 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
403 FREENECT_RESOLUTION_MEDIUM,
405 ? FREENECT_VIDEO_IR_8BIT
406 : FREENECT_VIDEO_BAYER
411 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
415 const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
418 m_buf_depth.resize(frMode.width * frMode.height * 3);
419 m_buf_rgb.resize(frMode.width * frMode.height * 3);
428 freenect_set_video_buffer(f_dev, &
m_buf_rgb[0]);
431 freenect_set_depth_mode(
432 f_dev, freenect_find_depth_mode(
433 FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
436 freenect_set_user(f_dev,
this);
438 if (freenect_start_depth(f_dev) < 0)
441 if (freenect_start_video(f_dev) < 0)
444 #endif // MRPT_HAS_KINECT_FREENECT
449 #if MRPT_HAS_KINECT_FREENECT
452 freenect_stop_depth(f_dev);
453 freenect_stop_video(f_dev);
454 freenect_close_device(f_dev);
458 if (f_ctx) freenect_shutdown(f_ctx);
460 #endif // MRPT_HAS_KINECT_FREENECT
470 #if MRPT_HAS_KINECT_FREENECT
475 freenect_stop_video(f_dev);
478 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
479 FREENECT_RESOLUTION_MEDIUM,
481 ? FREENECT_VIDEO_IR_8BIT
482 : FREENECT_VIDEO_BAYER
487 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
490 freenect_start_video(f_dev);
492 #endif // MRPT_HAS_KINECT_FREENECT
506 bool& hardware_error)
508 there_is_obs =
false;
509 hardware_error =
false;
511 #if MRPT_HAS_KINECT_FREENECT
513 using namespace std::chrono_literals;
514 const auto max_wait = 40ms;
517 m_latest_obs.hasPoints3D =
false;
518 m_latest_obs.hasRangeImage =
false;
519 m_latest_obs.hasIntensityImage =
false;
520 m_latest_obs.hasConfidenceImage =
false;
526 m_latest_obs_cs.lock();
527 m_tim_latest_rgb = 0;
528 m_tim_latest_depth = 0;
529 m_latest_obs_cs.unlock();
531 while (freenect_process_events(f_ctx) >= 0 &&
536 m_tim_latest_rgb != 0) &&
539 m_tim_latest_depth != 0)
558 m_latest_obs.hasRangeImage =
true;
559 m_latest_obs.range_is_depth =
true;
561 m_latest_obs_cs.lock();
564 m_latest_obs.rangeImage.setSize(
566 m_latest_obs.rangeImage.fill(0);
567 m_latest_obs_cs.unlock();
571 if (!there_is_obs)
return;
576 m_latest_obs_cs.lock();
577 m_out_obs.
swap(m_latest_obs);
578 m_latest_obs_cs.unlock();
651 bool& hardware_error)
659 double acc_x = 0, acc_y = 0, acc_z = 0;
660 bool has_good_acc =
false;
662 #if MRPT_HAS_KINECT_FREENECT
664 freenect_update_tilt_state(f_dev);
665 freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
670 freenect_get_mks_accel(state, &lx, &ly, &lz);
707 [maybe_unused]]
const std::string& directory)
728 #if MRPT_HAS_KINECT_FREENECT
729 freenect_set_tilt_degs(f_dev, angle);
737 #if MRPT_KINECT_WITH_FREENECT
738 freenect_update_tilt_state(f_dev);
739 freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
740 return freenect_get_tilt_degs(ts);
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content,...
int m_user_device_number
Number of device to open (0:first,...)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
uint16_t[KINECT_RANGES_TABLE_LEN] TDepth2RangeArray
A type for an array that converts raw depth to ranges in millimeters.
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]",...
mrpt::poses::CPose3D m_sensorPoseOnRobot
size_t m_preview_window_decimation
If preview is enabled, only show 1 out of N images.
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
void calculate_range2meters()
Compute m_range2meters at construction.
mrpt::gui::CDisplayWindow::Ptr m_win_range
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
double fx() const
Get the value of the focal length x-value (in pixels).
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string §ion) override
See the class documentation at the top for expected parameters.
void getNextObservation(mrpt::obs::CObservation3DRangeScan &out_obs, bool &there_is_obs, bool &hardware_error)
The main data retrieving function, to be called after calling loadConfig() and initialize().
mrpt::math::TPose3DQuat asTPose() const
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
uint32_t ncols
Camera resolution.
void open()
Try to open the camera (set all the parameters before calling this) - users may also call initialize(...
size_t m_preview_decim_counter_rgb
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Contains classes for various device interfaces.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
#define KINECT_RANGES_TABLE_MASK
std::vector< uint8_t > m_buf_rgb
void setPathForExternalImages(const std::string &directory) override
Set the path where to save off-rawlog image files (this class DOES take into account this path).
#define THROW_EXCEPTION(msg)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation,...
void unprojectInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams=T3DPointsProjectionParams(), const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and ...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
void rangeImage_setSize(const int HEIGHT, const int WIDTH)
Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
This namespace contains representation of robot actions and observations.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
~CKinect() override
Default ctor.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
double fy() const
Get the value of the focal length y-value (in pixels).
TVideoChannel m_video_channel
The video channel to open: RGB or IR.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
std::shared_ptr< mrpt::obs ::CObservationIMU > Ptr
mrpt::img::TCamera m_cameraParamsRGB
Params for the RGB camera.
void loadFromMemoryBuffer(unsigned int width, unsigned int height, bool color, unsigned char *rawpixels, bool swapRedBlue=false)
Reads the image from raw pixels buffer in memory.
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
double leave(const std::string_view &func_name) noexcept
End of a named section.
void loadFromConfigFile(const std::string §ion, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the same format that used in saveToConfigFile().
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Structure to hold the parameters of a pinhole stereo camera model.
This class allows loading and storing values and vectors of different types from a configuration text...
@ IMU_Z_ACC
z-axis acceleration (local/vehicle frame) (m/sec2)
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void appendObservations(const std::vector< mrpt::serialization::CSerializable::Ptr > &obj)
This method must be called by derived classes to enqueue a new observation in the list to be returned...
size_t m_preview_decim_counter_range
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
bool hasIntensityImage
true means the field intensityImage contains valid data
double getTiltAngleDegrees()
bool hasRangeImage
true means the field rangeImage contains valid data
void enter(const std::string_view &func_name) noexcept
Start of a named section.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
@ IMU_X_ACC
x-axis acceleration (local/vehicle frame) (m/sec2)
std::string m_sensorLabel
See CGenericSensor.
double m_maxRange
Sensor max range (meters)
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
constexpr double DEG2RAD(const double x)
Degrees to radians
bool m_preview_window
Show preview window while grabbing.
A class for storing images as grayscale or RGB bitmaps.
std::vector< bool > dataIsPresent
Each entry in this vector is true if the corresponding data index contains valid data (the IMU unit s...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
mrpt::img::TCamera m_cameraParamsDepth
Params for the Depth camera.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
@ CH_IR
Infrarred (IR) channel.
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).
bool m_grab_image
Default: all true.
void resize(size_t row, size_t col)
@ IMU_Y_ACC
y-axis acceleration (local/vehicle frame) (m/sec2)
mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth
See mrpt::obs::CObservation3DRangeScan for a diagram of this pose.
mrpt::gui::CDisplayWindow::Ptr m_win_int
TVideoChannel
RGB or IR video channel identifiers.
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot.
@ CH_VISIBLE
Grayscale or RGB visible channel of the camera sensor.
int m_initial_tilt_angle
Set Kinect tilt to an initial deegre (it should be take in account in the sensor pose by the user)
void setTiltAngleDegrees(double angle)
Change tilt angle.
This base provides a set of functions for maths stuff.
#define KINECT_RANGES_TABLE_LEN
void setVideoChannel(const TVideoChannel vch)
Changes the video channel to open (RGB or IR) - you can call this method before start grabbing or in ...
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
Scalar & coeffRef(int r, int c)
bool isOpen() const
Whether there is a working connection to the sensor.
TDepth2RangeArray m_range2meters
The table raw depth -> range in meters.
void close()
Close the Connection to the sensor (not need to call it manually unless desired for some reason,...
std::vector< uint8_t > m_buf_depth
Temporary buffers for image grabbing.
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
Builds a visualization from the rangeImage.
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020 | |