18 #include <mrpt/otherlibs/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;
65 m_range2meters[i] = k3 * tanf(i / k2 + k1);
69 m_range2meters[i] = 1.0f / (i * (-0.0030711016) + 3.3309495161);
73 m_range2meters[0] = 0;
81 : m_sensorPoseOnRobot(),
82 m_relativePoseIntensityWRTDepth(
121 "MRPT was compiled without support for Kinect. Please, rebuild it.");
141 bool thereIs, hwError;
144 std::make_shared<CObservation3DRangeScan>();
159 vector<CSerializable::Ptr> objs;
161 objs.push_back(newObs);
179 configSource.
read_float(iniSection,
"pose_x", 0),
180 configSource.
read_float(iniSection,
"pose_y", 0),
181 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);
268 auto*
obj =
reinterpret_cast<CKinect*
>(freenect_get_user(dev));
271 std::lock_guard<std::mutex> lock(
obj->internal_latest_obs_cs());
275 obs.range_is_depth =
true;
277 #ifdef KINECT_PROFILE_MEM_ALLOC 278 alloc_tim.
enter(
"depth_cb alloc");
282 obs.rangeImage_setSize(frMode.height, frMode.width);
284 #ifdef KINECT_PROFILE_MEM_ALLOC 285 alloc_tim.
leave(
"depth_cb alloc");
289 for (
int r = 0;
r < frMode.height;
r++)
290 for (
int c = 0;
c < frMode.width;
c++)
298 obj->internal_tim_latest_depth() = timestamp;
301 void rgb_cb(freenect_device* dev,
void* img_data,
uint32_t timestamp)
303 auto*
obj =
reinterpret_cast<CKinect*
>(freenect_get_user(dev));
304 const freenect_frame_mode frMode = freenect_get_current_video_mode(dev);
307 std::lock_guard<std::mutex> lock(
obj->internal_latest_obs_cs());
310 #ifdef KINECT_PROFILE_MEM_ALLOC 311 alloc_tim.
enter(
"depth_rgb loadFromMemoryBuffer");
314 obs.hasIntensityImage =
true;
322 obs.intensityImageChannel =
324 obs.intensityImage.resize(
328 const cv::Mat src_img_bayer(
329 frMode.height, frMode.width, CV_8UC1, img_data, frMode.width);
331 cv::Mat& dst_img_RGB = obs.intensityImage.asCvMatRef();
334 cv::cvtColor(src_img_bayer, dst_img_RGB, cv::COLOR_BayerGB2BGR);
343 obs.intensityImage.loadFromMemoryBuffer(
344 frMode.width, frMode.height,
346 reinterpret_cast<unsigned char*>(img_data));
351 #ifdef KINECT_PROFILE_MEM_ALLOC 352 alloc_tim.
leave(
"depth_rgb loadFromMemoryBuffer");
355 obj->internal_tim_latest_rgb() = timestamp;
358 #endif // MRPT_HAS_KINECT_FREENECT 368 #if MRPT_HAS_KINECT_FREENECT // ----> libfreenect 370 if (freenect_init(f_ctx_ptr,
nullptr) < 0)
373 freenect_set_log_level(
382 int nr_devices = freenect_num_devices(f_ctx);
395 freenect_set_led(f_dev, LED_RED);
396 freenect_set_depth_callback(f_dev, depth_cb);
397 freenect_set_video_callback(f_dev, rgb_cb);
400 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
401 FREENECT_RESOLUTION_MEDIUM,
403 ? FREENECT_VIDEO_IR_8BIT
404 : FREENECT_VIDEO_BAYER
409 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
413 const freenect_frame_mode frMode = freenect_get_current_video_mode(f_dev);
416 m_buf_depth.resize(frMode.width * frMode.height * 3);
417 m_buf_rgb.resize(frMode.width * frMode.height * 3);
426 freenect_set_video_buffer(f_dev, &
m_buf_rgb[0]);
429 freenect_set_depth_mode(
430 f_dev, freenect_find_depth_mode(
431 FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_10BIT));
434 freenect_set_user(f_dev,
this);
436 if (freenect_start_depth(f_dev) < 0)
439 if (freenect_start_video(f_dev) < 0)
442 #endif // MRPT_HAS_KINECT_FREENECT 447 #if MRPT_HAS_KINECT_FREENECT 450 freenect_stop_depth(f_dev);
451 freenect_stop_video(f_dev);
452 freenect_close_device(f_dev);
456 if (f_ctx) freenect_shutdown(f_ctx);
458 #endif // MRPT_HAS_KINECT_FREENECT 468 #if MRPT_HAS_KINECT_FREENECT 473 freenect_stop_video(f_dev);
476 const freenect_frame_mode desiredFrMode = freenect_find_video_mode(
477 FREENECT_RESOLUTION_MEDIUM,
479 ? FREENECT_VIDEO_IR_8BIT
480 : FREENECT_VIDEO_BAYER
485 if (freenect_set_video_mode(f_dev, desiredFrMode) < 0)
488 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 _out_obs.
swap(m_latest_obs);
578 m_latest_obs_cs.unlock();
654 bool& hardware_error)
662 double acc_x = 0, acc_y = 0, acc_z = 0;
663 bool has_good_acc =
false;
665 #if MRPT_HAS_KINECT_FREENECT 667 freenect_update_tilt_state(f_dev);
668 freenect_raw_tilt_state* state = freenect_get_tilt_state(f_dev);
673 freenect_get_mks_accel(state, &lx, &ly, &lz);
731 #if MRPT_HAS_KINECT_FREENECT 732 freenect_set_tilt_degs(f_dev, angle);
742 #if MRPT_KINECT_WITH_FREENECT 743 freenect_update_tilt_state(f_dev);
744 freenect_raw_tilt_state* ts = freenect_get_tilt_state(f_dev);
745 return freenect_get_tilt_degs(ts);
mrpt::img::TCamera m_cameraParamsRGB
Params for the RGB camera.
mrpt::img::TCamera m_cameraParamsDepth
Params for the Depth camera.
void doProcess() override
To be called at a high rate (>XX Hz), this method populates the internal buffer of received observati...
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
std::vector< bool > dataIsPresent
Each entry in this vector is true if the corresponding data index contains valid data (the IMU unit s...
unsigned __int16 uint16_t
void resize(size_t row, size_t col)
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::vector< double > rawMeasurements
The accelerometer and/or gyroscope measurements taken by the IMU at the given timestamp.
TVideoChannel m_video_channel
The video channel to open: RGB or IR.
#define THROW_EXCEPTION(msg)
double fx() const
Get the value of the focal length x-value (in pixels).
double getTiltAngleDegrees()
std::string m_sensorLabel
See CGenericSensor.
mrpt::gui::CDisplayWindow::Ptr m_win_int
size_t m_preview_window_decimation
If preview is enabled, only show 1 out of N images.
double DEG2RAD(const double x)
Degrees to radians.
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
void setTiltAngleDegrees(double angle)
Change tilt angle.
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
double m_maxRange
Sensor max range (meters)
double fy() const
Get the value of the focal length y-value (in pixels).
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
mrpt::math::TPose3DQuat asTPose() const
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
GLint GLint GLsizei GLsizei GLsizei depth
y-axis acceleration (local/vehicle frame) (m/sec2)
This class stores measurements from an Inertial Measurement Unit (IMU) (attitude estimation, raw gyroscope and accelerometer values), altimeters or magnetometers.
z-axis acceleration (local/vehicle frame) (m/sec2)
GLsizei GLsizei GLuint * obj
#define KINECT_RANGES_TABLE_LEN
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string §ion) override
See the class documentation at the top for expected parameters.
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_rgb
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define KINECT_RANGES_TABLE_MASK
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...
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...
void calculate_range2meters()
Compute m_range2meters at construction.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
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 swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
TDepth2RangeArray m_range2meters
The table raw depth -> range in meters.
This namespace contains representation of robot actions and observations.
void initialize() override
Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different par...
bool hasRangeImage
true means the field rangeImage contains valid data
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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 class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
GLsizei const GLchar ** string
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 IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int m_user_device_number
Number of device to open (0:first,...)
void setVideoChannel(const TVideoChannel vch)
Changes the video channel to open (RGB or IR) - you can call this method before start grabbing or in ...
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().
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
~CKinect() override
Default ctor.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
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::system::TTimeStamp timestamp
The associated UTC time-stamp.
GLdouble GLdouble GLdouble r
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
size_t m_preview_decim_counter_range
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
float[KINECT_RANGES_TABLE_LEN] TDepth2RangeArray
A type for an array that converts raw depth to ranges in meters.
mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth
See mrpt::obs::CObservation3DRangeScan for a diagram of this pose.
bool hasIntensityImage
true means the field intensityImage contains valid data
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...
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
bool isOpen() const
Whether there is a working connection to the sensor.
bool m_preview_window
Show preview window while grabbing.
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
std::vector< uint8_t > m_buf_depth
Temporary buffers for image grabbing.
uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1)
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
unsigned __int32 uint32_t
This template class provides the basic functionality for a general 2D any-size, resizable container o...
x-axis acceleration (local/vehicle frame) (m/sec2)
uint32_t ncols
Camera resolution.
mrpt::gui::CDisplayWindow::Ptr m_win_range
void enter(const std::string_view &func_name)
Start of a named section.
bool m_grab_image
Default: all true.
Structure to hold the parameters of a pinhole stereo camera model.
size_t m_preview_decim_counter_rgb
mrpt::poses::CPose3D m_sensorPoseOnRobot
double leave(const std::string_view &func_name)
End of a named section.
Grayscale or RGB visible channel of the camera sensor.
A class for storing images as grayscale or RGB bitmaps.
TVideoChannel
RGB or IR video channel identifiers.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void open()
Try to open the camera (set all the parameters before calling this) - users may also call initialize(...