ivcam-device.cpp
Go to the documentation of this file.
00001 // License: Apache 2.0. See LICENSE file in root directory.
00002 // Copyright(c) 2016 Intel Corporation. All Rights Reserved.
00003 
00004 #define _USE_MATH_DEFINES
00005 #include <math.h>
00006 #include <limits>
00007 #include <climits>
00008 #include <algorithm>
00009 
00010 #include "image.h"
00011 #include "ivcam-private.h"
00012 #include "ivcam-device.h"
00013 
00014 namespace rsimpl
00015 {
00016     rs_intrinsics MakeDepthIntrinsics(const ivcam::camera_calib_params & c, const int2 & dims)
00017     {
00018         return{ dims.x, dims.y, (c.Kc[0][2] * 0.5f + 0.5f) * dims.x, (c.Kc[1][2] * 0.5f + 0.5f) * dims.y, c.Kc[0][0] * 0.5f * dims.x, c.Kc[1][1] * 0.5f * dims.y,
00019             RS_DISTORTION_INVERSE_BROWN_CONRADY, {c.Invdistc[0], c.Invdistc[1], c.Invdistc[2], c.Invdistc[3], c.Invdistc[4]} };
00020     }
00021 
00022     rs_intrinsics MakeColorIntrinsics(const ivcam::camera_calib_params & c, const int2 & dims)
00023     {
00024         rs_intrinsics intrin = { dims.x, dims.y, c.Kt[0][2] * 0.5f + 0.5f, c.Kt[1][2] * 0.5f + 0.5f, c.Kt[0][0] * 0.5f, c.Kt[1][1] * 0.5f, RS_DISTORTION_NONE, {} };
00025         if (dims.x * 3 == dims.y * 4) // If using a 4:3 aspect ratio, adjust intrinsics (defaults to 16:9)
00026         {
00027             intrin.fx *= 4.0f / 3;
00028             intrin.ppx *= 4.0f / 3;
00029             intrin.ppx -= 1.0f / 6;
00030         }
00031         intrin.fx *= dims.x;
00032         intrin.fy *= dims.y;
00033         intrin.ppx *= dims.x;
00034         intrin.ppy *= dims.y;
00035         return intrin;
00036     }
00037 
00038     void update_supported_options(uvc::device& dev,
00039         const uvc::extension_unit depth_xu,
00040         const std::vector <std::pair<rs_option, char>> options,
00041         std::vector<supported_option>& supported_options)
00042     {
00043         for (auto p : options)
00044         {
00045             int min, max, step, def;
00046             get_extension_control_range(dev, depth_xu, p.second, &min, &max, &step, &def);
00047             supported_option so;
00048             so.option = p.first;
00049             so.min = min;
00050             so.max = max;
00051             so.step = step;
00052             so.def = def;
00053             supported_options.push_back(so);
00054         }
00055     }
00056 
00057     iv_camera::iv_camera(std::shared_ptr<uvc::device> device, const static_device_info & info, const ivcam::camera_calib_params & calib) :
00058         rs_device_base(device, info),
00059         base_calibration(calib)
00060     {
00061     }
00062 
00063     void iv_camera::start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex& mutex)
00064     {
00065         rs_device_base::start_fw_logger(fw_log_op_code, grab_rate_in_ms, mutex);
00066     }
00067 
00068     void iv_camera::stop_fw_logger()
00069     {
00070         rs_device_base::stop_fw_logger();
00071     }
00072 
00073     void iv_camera::on_before_start(const std::vector<subdevice_mode_selection> & /*selected_modes*/)
00074     {
00075     }
00076 
00077     rs_stream iv_camera::select_key_stream(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes)
00078     {
00079         int fps[RS_STREAM_NATIVE_COUNT] = {}, max_fps = 0;
00080         for (const auto & m : selected_modes)
00081         {
00082             for (const auto & output : m.get_outputs())
00083             {
00084                 fps[output.first] = m.mode.fps;
00085                 max_fps = std::max(max_fps, m.mode.fps);
00086             }
00087         }
00088 
00089         // Prefer to sync on depth or infrared, but select the stream running at the fastest framerate
00090         for (auto s : { RS_STREAM_DEPTH, RS_STREAM_INFRARED2, RS_STREAM_INFRARED, RS_STREAM_COLOR })
00091         {
00092             if (fps[s] == max_fps) return s;
00093         }
00094         return RS_STREAM_DEPTH;
00095     }
00096 
00097     void iv_camera::set_options(const rs_option options[], size_t count, const double values[])
00098     {
00099         std::vector<rs_option>  base_opt;
00100         std::vector<double>     base_opt_val;
00101 
00102         for (size_t i = 0; i < count; ++i)
00103         {
00104 
00105             if (uvc::is_pu_control(options[i]))
00106             {
00107                 // Disabling auto-setting controls, if needed
00108                 switch (options[i])
00109                 {
00110                 case RS_OPTION_COLOR_WHITE_BALANCE:     disable_auto_option( 0, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE); break;
00111                 case RS_OPTION_COLOR_EXPOSURE:          disable_auto_option( 0, RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE); break;
00112                 default:  break;
00113                 }
00114 
00115                 uvc::set_pu_control_with_retry(get_device(), 0, options[i], static_cast<int>(values[i]));
00116                 continue;
00117             }
00118 
00119             switch (options[i])
00120             {
00121             case RS_OPTION_F200_LASER_POWER:          ivcam::set_laser_power(get_device(), static_cast<uint8_t>(values[i])); break;
00122             case RS_OPTION_F200_ACCURACY:             ivcam::set_accuracy(get_device(), static_cast<uint8_t>(values[i])); break;
00123             case RS_OPTION_F200_MOTION_RANGE:         ivcam::set_motion_range(get_device(), static_cast<uint8_t>(values[i])); break;
00124             case RS_OPTION_F200_FILTER_OPTION:        ivcam::set_filter_option(get_device(), static_cast<uint8_t>(values[i])); break;
00125             case RS_OPTION_F200_CONFIDENCE_THRESHOLD: ivcam::set_confidence_threshold(get_device(), static_cast<uint8_t>(values[i])); break;
00126 
00127             default: 
00128                 base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
00129             }
00130         }
00131 
00132         //Set common options
00133         if (!base_opt.empty())
00134             rs_device_base::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00135     }
00136 
00137     void iv_camera::get_options(const rs_option options[], size_t count, double values[])
00138     {
00139         std::vector<rs_option>  base_opt;
00140         std::vector<size_t>     base_opt_index;
00141         std::vector<double>     base_opt_val;
00142 
00143         for (size_t i = 0; i < count; ++i)
00144         {
00145             LOG_INFO("Reading option " << options[i]);
00146 
00147             if (uvc::is_pu_control(options[i]))
00148                 throw std::logic_error(to_string() << __FUNCTION__ << " Option " << options[i] << " must be processed by a concrete class");
00149 
00150             uint8_t val = 0;
00151             switch (options[i])
00152             {
00153             case RS_OPTION_F200_LASER_POWER:          ivcam::get_laser_power(get_device(), val); values[i] = val; break;
00154             case RS_OPTION_F200_ACCURACY:             ivcam::get_accuracy(get_device(), val); values[i] = val; break;
00155             case RS_OPTION_F200_MOTION_RANGE:         ivcam::get_motion_range(get_device(), val); values[i] = val; break;
00156             case RS_OPTION_F200_FILTER_OPTION:        ivcam::get_filter_option(get_device(), val); values[i] = val; break;
00157             case RS_OPTION_F200_CONFIDENCE_THRESHOLD: ivcam::get_confidence_threshold(get_device(), val); values[i] = val; break;
00158 
00159             default: 
00160                 base_opt.push_back(options[i]); base_opt_index.push_back(i);
00161                /* LOG_WARNING("Cannot get " << options[i] << " on " << get_name());
00162                 throw std::logic_error("Option unsupported");*/
00163                 break;
00164             }
00165         }
00166         //Retrieve the common options
00167         if (base_opt.size())
00168         {
00169             base_opt_val.resize(base_opt.size());
00170             rs_device_base::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00171         }
00172 
00173         // Merge the local data with values obtained by base class
00174         for (auto i : base_opt_index)
00175             values[i] = base_opt_val[i];
00176     }
00177 
00178     // TODO: This may need to be modified for thread safety
00179     class rolling_timestamp_reader : public frame_timestamp_reader
00180     {
00181         bool started;
00182         int64_t total;
00183         int last_timestamp;
00184         int64_t counter = 0;
00185     public:
00186         rolling_timestamp_reader() : started(), total() {}
00187 
00188         bool validate_frame(const subdevice_mode & mode, const void * frame) override
00189         {
00190             // Validate that at least one byte of the image is nonzero
00191             for (const uint8_t * it = (const uint8_t *)frame, *end = it + mode.pf.get_image_size(mode.native_dims.x, mode.native_dims.y); it != end; ++it)
00192             {
00193                 if (*it)
00194                 {
00195                     return true;
00196                 }
00197             }
00198 
00199             // F200 and SR300 can sometimes produce empty frames shortly after starting, ignore them
00200             LOG_INFO("Subdevice " << mode.subdevice << " produced empty frame");
00201             return false;
00202         }
00203 
00204         double get_frame_timestamp(const subdevice_mode & /*mode*/, const void * frame, double /*actual_fps*/) override
00205         {
00206             // Timestamps are encoded within the first 32 bits of the image
00207             int rolling_timestamp = *reinterpret_cast<const int32_t *>(frame);
00208 
00209             if (!started)
00210             {
00211                 last_timestamp = rolling_timestamp;
00212                 started = true;
00213             }
00214 
00215             const int delta = rolling_timestamp - last_timestamp; // NOTE: Relies on undefined behavior: signed int wraparound
00216             last_timestamp = rolling_timestamp;
00217             total += delta;
00218             const int timestamp = static_cast<int>(total / 100000);
00219             return timestamp;
00220         }
00221         unsigned long long get_frame_counter(const subdevice_mode & /*mode*/, const void * /*frame*/) override
00222         {
00223             return ++counter;
00224         }
00225     };
00226 
00227     std::vector<std::shared_ptr<frame_timestamp_reader>> iv_camera::create_frame_timestamp_readers() const
00228     {
00229         auto the_reader = std::make_shared<rolling_timestamp_reader>(); // single shared timestamp reader for all subdevices
00230         return { the_reader, the_reader }; // clone the reference for color and depth
00231     }
00232 
00233 } // namespace rsimpl::f200


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39