00001
00002
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)
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> & )
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
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
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
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
00162
00163 break;
00164 }
00165 }
00166
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
00174 for (auto i : base_opt_index)
00175 values[i] = base_opt_val[i];
00176 }
00177
00178
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
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
00200 LOG_INFO("Subdevice " << mode.subdevice << " produced empty frame");
00201 return false;
00202 }
00203
00204 double get_frame_timestamp(const subdevice_mode & , const void * frame, double ) override
00205 {
00206
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;
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 & , const void * ) 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>();
00230 return { the_reader, the_reader };
00231 }
00232
00233 }