00001
00002
00003
00004 #include "stream.h"
00005 #include "sync.h"
00006 #include "image.h"
00007 #include <algorithm>
00008 #include <tuple>
00009
00010 using namespace rsimpl;
00011
00012 rs_extrinsics stream_interface::get_extrinsics_to(const rs_stream_interface & other) const
00013 {
00014 if (!validator.validate_extrinsics(stream, other.get_stream_type()))
00015 {
00016 throw std::runtime_error(to_string() << "The extrinsic from " << get_stream_type() << " to " << other.get_stream_type() << " is not valid");
00017 }
00018
00019 auto& r = dynamic_cast<const stream_interface&>(other);
00020 auto from = get_pose(), to = r.get_pose();
00021 if(from == to) return {{1,0,0,0,1,0,0,0,1},{0,0,0}};
00022 auto transform = inverse(from) * to;
00023 rs_extrinsics extrin;
00024 (float3x3 &)extrin.rotation = transform.orientation;
00025 (float3 &)extrin.translation = transform.position;
00026 return extrin;
00027 }
00028
00029 native_stream::native_stream(device_config & config, rs_stream stream, calibration_validator in_validator) : stream_interface(in_validator, stream), config(config)
00030 {
00031 for(auto & subdevice_mode : config.info.subdevice_modes)
00032 {
00033 for(auto pad_crop : subdevice_mode.pad_crop_options)
00034 {
00035 for(auto & unpacker : subdevice_mode.pf.unpackers)
00036 {
00037 auto selection = subdevice_mode_selection(subdevice_mode, pad_crop, (int)(&unpacker - subdevice_mode.pf.unpackers.data()));
00038 if(selection.provides_stream(stream)) modes.push_back(selection);
00039 }
00040 }
00041 }
00042
00043 auto get_tuple = [stream](const subdevice_mode_selection & selection)
00044 {
00045 return std::make_tuple(-selection.get_width(), -selection.get_height(), -selection.get_framerate(), selection.get_format(stream));
00046 };
00047
00048 std::sort(begin(modes), end(modes), [get_tuple](const subdevice_mode_selection & a, const subdevice_mode_selection & b) { return get_tuple(a) < get_tuple(b); });
00049 auto it = std::unique(begin(modes), end(modes), [get_tuple](const subdevice_mode_selection & a, const subdevice_mode_selection & b) { return get_tuple(a) == get_tuple(b); });
00050 if(it != end(modes)) modes.erase(it, end(modes));
00051 }
00052
00053 void native_stream::get_mode(int mode, int * w, int * h, rs_format * f, int * fps) const
00054 {
00055 auto & selection = modes[mode];
00056 if(w) *w = selection.get_width();
00057 if(h) *h = selection.get_height();
00058 if(f) *f = selection.get_format(stream);
00059 if(fps) *fps = selection.get_framerate();
00060 }
00061
00062 bool native_stream::is_enabled() const
00063 {
00064 return (archive && archive->is_stream_enabled(stream)) || config.requests[stream].enabled;
00065 }
00066
00067 subdevice_mode_selection native_stream::get_mode() const
00068 {
00069 if(archive && archive->is_stream_enabled(stream)) return archive->get_mode(stream);
00070 if(config.requests[stream].enabled)
00071 {
00072 for(auto subdevice_mode : config.select_modes())
00073 {
00074 if(subdevice_mode.provides_stream(stream)) return subdevice_mode;
00075 }
00076 throw std::logic_error("no mode found");
00077 }
00078 throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00079 }
00080
00081 rs_intrinsics native_stream::get_intrinsics() const
00082 {
00083 if (!validator.validate_intrinsics(stream))
00084 {
00085 LOG_ERROR("The intrinsic of " << get_stream_type() << " is not valid");
00086 }
00087 const auto m = get_mode();
00088 return pad_crop_intrinsics(m.mode.native_intrinsics, m.pad_crop);
00089 }
00090
00091 rs_intrinsics native_stream::get_rectified_intrinsics() const
00092 {
00093 if (!validator.validate_intrinsics(stream))
00094 {
00095 throw std::runtime_error(to_string() << "The intrinsic of " << get_stream_type() << " is not valid");
00096 }
00097 const auto m = get_mode();
00098 if(m.mode.rect_modes.empty()) return get_intrinsics();
00099 return pad_crop_intrinsics(m.mode.rect_modes[0], m.pad_crop);
00100 }
00101
00102 double native_stream::get_frame_metadata(rs_frame_metadata frame_metadata) const
00103 {
00104 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00105 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00106 return archive->get_frame_metadata(stream, frame_metadata);
00107 }
00108
00109 bool native_stream::supports_frame_metadata(rs_frame_metadata frame_metadata) const
00110 {
00111 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00112 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00113 return archive->supports_frame_metadata(stream, frame_metadata);
00114 }
00115
00116 unsigned long long native_stream::get_frame_number() const
00117 {
00118 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00119 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00120 return archive->get_frame_number(stream);
00121 }
00122
00123 double native_stream::get_frame_timestamp() const
00124 {
00125 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00126 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00127 return archive->get_frame_timestamp(stream);
00128 }
00129
00130 long long native_stream::get_frame_system_time() const
00131 {
00132 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00133 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00134 return archive->get_frame_system_time(stream);
00135 }
00136
00137 const uint8_t * native_stream::get_frame_data() const
00138 {
00139 if(!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00140 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00141 return (const uint8_t *) archive->get_frame_data(stream);
00142 }
00143
00144 int native_stream::get_frame_stride() const
00145 {
00146 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00147 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00148 return archive->get_frame_stride(stream);
00149 }
00150
00151 int native_stream::get_frame_bpp() const
00152 {
00153 if (!is_enabled()) throw std::runtime_error(to_string() << "stream not enabled: " << stream);
00154 if (!archive) throw std::runtime_error(to_string() << "streaming not started!");
00155 return archive->get_frame_bpp(stream);
00156 }
00157
00158 const uint8_t * point_stream::get_frame_data() const
00159 {
00160 if(image.empty() || number != get_frame_number())
00161 {
00162 image.resize(get_image_size(get_intrinsics().width, get_intrinsics().height, get_format()));
00163
00164 if(source.get_format() == RS_FORMAT_Z16)
00165 {
00166 deproject_z(reinterpret_cast<float *>(image.data()), get_intrinsics(), reinterpret_cast<const uint16_t *>(source.get_frame_data()), get_depth_scale());
00167 }
00168 else if(source.get_format() == RS_FORMAT_DISPARITY16)
00169 {
00170 deproject_disparity(reinterpret_cast<float *>(image.data()), get_intrinsics(), reinterpret_cast<const uint16_t *>(source.get_frame_data()), get_depth_scale());
00171 }
00172 else assert(false && "Cannot deproject image from a non-depth format");
00173
00174 number = get_frame_number();
00175 }
00176 return image.data();
00177 }
00178
00179 const uint8_t * rectified_stream::get_frame_data() const
00180 {
00181
00182 if(get_pose() == source.get_pose() && get_intrinsics() == source.get_intrinsics()) return source.get_frame_data();
00183
00184 if(image.empty() || number != get_frame_number())
00185 {
00186 if(table.empty()) table = compute_rectification_table(get_intrinsics(), get_extrinsics_to(source), source.get_intrinsics());
00187 image.resize(get_image_size(get_intrinsics().width, get_intrinsics().height, get_format()));
00188 rectify_image(image.data(), table, source.get_frame_data(), get_format());
00189 number = get_frame_number();
00190 }
00191 return image.data();
00192 }
00193
00194 const uint8_t * aligned_stream::get_frame_data() const
00195 {
00196 if(image.empty() || number != get_frame_number())
00197 {
00198 image.resize(get_image_size(get_intrinsics().width, get_intrinsics().height, get_format()));
00199 memset(image.data(), from.get_format() == RS_FORMAT_DISPARITY16 ? 0xFF : 0x00, image.size());
00200 if(from.get_format() == RS_FORMAT_Z16)
00201 {
00202 align_z_to_other(image.data(), (const uint16_t *)from.get_frame_data(), from.get_depth_scale(), from.get_intrinsics(), from.get_extrinsics_to(to), to.get_intrinsics());
00203 }
00204 else if(from.get_format() == RS_FORMAT_DISPARITY16)
00205 {
00206 align_disparity_to_other(image.data(), (const uint16_t *)from.get_frame_data(), from.get_depth_scale(), from.get_intrinsics(), from.get_extrinsics_to(to), to.get_intrinsics());
00207 }
00208 else if(to.get_format() == RS_FORMAT_Z16)
00209 {
00210 align_other_to_z(image.data(), (const uint16_t *)to.get_frame_data(), to.get_depth_scale(), to.get_intrinsics(), to.get_extrinsics_to(from), from.get_intrinsics(), from.get_frame_data(), from.get_format());
00211 }
00212 else if(to.get_format() == RS_FORMAT_DISPARITY16)
00213 {
00214 align_other_to_disparity(image.data(), (const uint16_t *)to.get_frame_data(), to.get_depth_scale(), to.get_intrinsics(), to.get_extrinsics_to(from), from.get_intrinsics(), from.get_frame_data(), from.get_format());
00215 }
00216 else assert(false && "Cannot align two images if neither have depth data");
00217 number = get_frame_number();
00218 }
00219 return image.data();
00220 }