00001
00002
00003 #pragma once
00004 #ifndef LIBREALSENSE_UNITTESTS_COMMON_H
00005 #define LIBREALSENSE_UNITTESTS_COMMON_H
00006
00007 #include "catch/catch.hpp"
00008 #include <librealsense/rs.h>
00009 #include <limits>
00010 #include <cmath>
00011 #include <cassert>
00012 #include <thread>
00013 #include <map>
00014 #include <mutex>
00015 #include <condition_variable>
00016 #include <atomic>
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifdef WIN32
00026 #define NOEXCEPT_FALSE
00027 #else
00028 #define NOEXCEPT_FALSE noexcept(false)
00029 #endif
00030
00031
00032 class require_error
00033 {
00034 std::string message;
00035 bool validate_error_message;
00036 rs_error * err;
00037 public:
00038 require_error(std::string message,bool message_validation=true) : message(move(message)), validate_error_message(message_validation), err() {}
00039 require_error(const require_error &) = delete;
00040 ~require_error() NOEXCEPT_FALSE
00041 {
00042 assert(!std::uncaught_exception());
00043 REQUIRE(err != nullptr);
00044 if (validate_error_message)
00045 {
00046 REQUIRE(rs_get_error_message(err) == std::string(message));
00047 }
00048 }
00049 require_error & operator = (const require_error &) = delete;
00050 operator rs_error ** () { return &err; }
00051 };
00052
00053
00054 class require_no_error
00055 {
00056 rs_error * err;
00057 public:
00058 require_no_error() : err() {}
00059 require_no_error(const require_error &) = delete;
00060 ~require_no_error() NOEXCEPT_FALSE
00061 {
00062 assert(!std::uncaught_exception());
00063 REQUIRE(rs_get_error_message(err) == rs_get_error_message(nullptr));
00064 REQUIRE(err == nullptr);
00065 }
00066 require_no_error & operator = (const require_no_error &) = delete;
00067 operator rs_error ** () { return &err; }
00068 };
00069
00070
00071
00072 class safe_context
00073 {
00074 rs_context * context;
00075 safe_context(int) : context() {}
00076 public:
00077 safe_context() : safe_context(1)
00078 {
00079 context = rs_create_context(RS_API_VERSION, require_no_error());
00080 REQUIRE(context != nullptr);
00081 }
00082
00083 ~safe_context()
00084 {
00085 if(context)
00086 {
00087 rs_delete_context(context, nullptr);
00088 }
00089 }
00090
00091 bool operator == (const safe_context& other) const { return context == other.context; }
00092
00093 operator rs_context * () const { return context; }
00094 };
00095
00096
00097
00098 inline float dot_product(const float (& a)[3], const float (& b)[3])
00099 {
00100 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
00101 }
00102
00103
00104 inline float vector_length(const float (& v)[3])
00105 {
00106 return std::sqrt(dot_product(v, v));
00107 }
00108
00109
00110 inline void require_cross_product(const float (& r)[3], const float (& a)[3], const float (& b)[3])
00111 {
00112 REQUIRE( r[0] == Approx(a[1]*b[2] - a[2]*b[1]) );
00113 REQUIRE( r[1] == Approx(a[2]*b[0] - a[0]*b[2]) );
00114 REQUIRE( r[2] == Approx(a[0]*b[1] - a[1]*b[0]) );
00115 }
00116
00117
00118 inline void require_zero_vector(const float (& vector)[3])
00119 {
00120 for(int i=1; i<3; ++i) REQUIRE( vector[i] == 0.0f );
00121 }
00122
00123
00124 inline void require_transposed(const float (& a)[9], const float (& b)[9])
00125 {
00126 REQUIRE( a[0] == Approx(b[0]) );
00127 REQUIRE( a[1] == Approx(b[3]) );
00128 REQUIRE( a[2] == Approx(b[6]) );
00129 REQUIRE( a[3] == Approx(b[1]) );
00130 REQUIRE( a[4] == Approx(b[4]) );
00131 REQUIRE( a[5] == Approx(b[7]) );
00132 REQUIRE( a[6] == Approx(b[2]) );
00133 REQUIRE( a[7] == Approx(b[5]) );
00134 REQUIRE( a[8] == Approx(b[8]) );
00135 }
00136
00137
00138 inline void require_rotation_matrix(const float (& matrix)[9])
00139 {
00140 const float row0[] = {matrix[0], matrix[3], matrix[6]};
00141 const float row1[] = {matrix[1], matrix[4], matrix[7]};
00142 const float row2[] = {matrix[2], matrix[5], matrix[8]};
00143 REQUIRE( dot_product(row0, row0) == Approx(1) );
00144 REQUIRE( dot_product(row1, row1) == Approx(1) );
00145 REQUIRE( dot_product(row2, row2) == Approx(1) );
00146 REQUIRE( dot_product(row0, row1) == Approx(0) );
00147 REQUIRE( dot_product(row1, row2) == Approx(0) );
00148 REQUIRE( dot_product(row2, row0) == Approx(0) );
00149 require_cross_product(row0, row1, row2);
00150 require_cross_product(row0, row1, row2);
00151 require_cross_product(row0, row1, row2);
00152 }
00153
00154
00155 inline void require_identity_matrix(const float (& matrix)[9])
00156 {
00157 static const float identity_matrix_3x3[] = {1,0,0, 0,1,0, 0,0,1};
00158 for(int i=0; i<9; ++i) REQUIRE( matrix[i] == identity_matrix_3x3[i] );
00159 }
00160
00161 struct test_duration{
00162 bool is_start_time_initialized;
00163 bool is_end_time_initialized;
00164 std::chrono::high_resolution_clock::time_point start_time , end_time;
00165 uint32_t actual_frames_to_receive;
00166 uint32_t first_frame_to_capture;
00167 uint32_t frames_to_capture;
00168 };
00169
00170 inline void check_fps(double actual_fps, double configured_fps)
00171 {
00172 REQUIRE(actual_fps >= configured_fps * 0.9);
00173 }
00174
00175 struct stream_mode { rs_stream stream; int width, height; rs_format format; int framerate; };
00176
00177
00178 const int max_capture_time_sec = 3;
00179 const uint32_t max_frames_to_receive = 50;
00180
00181 inline void test_wait_for_frames(rs_device * device, std::initializer_list<stream_mode>& modes, std::map<rs_stream, test_duration>& duration_per_stream)
00182 {
00183 rs_start_device(device, require_no_error());
00184 REQUIRE( rs_is_device_streaming(device, require_no_error()) == 1 );
00185
00186 rs_wait_for_frames(device, require_no_error());
00187
00188 int lowest_fps_mode = std::numeric_limits<int>::max();
00189 for(auto & mode : modes)
00190 {
00191 REQUIRE( rs_is_stream_enabled(device, mode.stream, require_no_error()) == 1 );
00192 REQUIRE( rs_get_frame_data(device, mode.stream, require_no_error()) != nullptr );
00193 REQUIRE( rs_get_frame_timestamp(device, mode.stream, require_no_error()) >= 0 );
00194 if (mode.framerate < lowest_fps_mode) lowest_fps_mode = mode.framerate;
00195 }
00196
00197 std::vector<unsigned long long> last_frame_number;
00198 std::vector<unsigned long long> number_of_frames;
00199 last_frame_number.resize(RS_STREAM_COUNT);
00200 number_of_frames.resize(RS_STREAM_COUNT);
00201
00202 for (auto& elem : modes)
00203 {
00204 number_of_frames[elem.stream] = 0;
00205 last_frame_number[elem.stream] = 0;
00206 }
00207
00208 const auto actual_frames_to_receive = std::min(max_frames_to_receive, (uint32_t)(max_capture_time_sec* lowest_fps_mode));
00209 const auto first_frame_to_capture = (actual_frames_to_receive*0.1);
00210 const auto frames_to_capture = (actual_frames_to_receive - first_frame_to_capture);
00211
00212 for (auto i = 0u; i< actual_frames_to_receive; ++i)
00213 {
00214 rs_wait_for_frames(device, require_no_error());
00215
00216 if (i < first_frame_to_capture) continue;
00217
00218 for (auto & mode : modes)
00219 {
00220 if (rs_get_frame_timestamp(device, mode.stream, require_no_error()) > 0)
00221 {
00222 REQUIRE( rs_is_stream_enabled(device, mode.stream, require_no_error()) == 1);
00223 REQUIRE( rs_get_frame_data(device, mode.stream, require_no_error()) != nullptr);
00224 REQUIRE( rs_get_frame_timestamp(device, mode.stream, require_no_error()) >= 0);
00225
00226 auto frame_number = rs_get_frame_number(device, mode.stream, require_no_error());
00227 if (!duration_per_stream[mode.stream].is_end_time_initialized && last_frame_number[mode.stream] != frame_number)
00228 {
00229 last_frame_number[mode.stream] = frame_number;
00230 ++number_of_frames[mode.stream];
00231 }
00232
00233 if (!duration_per_stream[mode.stream].is_start_time_initialized && number_of_frames[mode.stream] >= 1)
00234 {
00235 duration_per_stream[mode.stream].start_time = std::chrono::high_resolution_clock::now();
00236 duration_per_stream[mode.stream].is_start_time_initialized = true;
00237 }
00238
00239 if (!duration_per_stream[mode.stream].is_end_time_initialized && (number_of_frames[mode.stream] > (0.9 * frames_to_capture)))
00240 {
00241 duration_per_stream[mode.stream].end_time = std::chrono::high_resolution_clock::now();
00242 auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(duration_per_stream[mode.stream].end_time - duration_per_stream[mode.stream].start_time).count();
00243 auto fps = ((double)number_of_frames[mode.stream] / duration) * 1000.;
00244
00245 duration_per_stream[mode.stream].is_end_time_initialized = true;
00246 }
00247 }
00248 }
00249 }
00250
00251 rs_stop_device(device, require_no_error());
00252 REQUIRE( rs_is_device_streaming(device, require_no_error()) == 0 );
00253 }
00254
00255
00256 static std::mutex m;
00257 static std::mutex cb_mtx;
00258 static std::condition_variable cv;
00259 static std::atomic<bool> stop_streaming;
00260 static int done;
00261 struct user_data{
00262 std::map<rs_stream, test_duration> duration_per_stream;
00263 std::map<rs_stream, unsigned> number_of_frames_per_stream;
00264 };
00265
00266
00267 inline void frame_callback(rs_device * dev, rs_frame_ref * frame, void * user)
00268 {
00269 std::lock_guard<std::mutex> lock(cb_mtx);
00270
00271 if (stop_streaming || (rs_get_detached_frame_timestamp(frame, require_no_error()) == 0))
00272 {
00273 rs_release_frame(dev, frame, require_no_error());
00274 return;
00275 }
00276
00277 auto data = (user_data*)user;
00278 bool stop = true;
00279
00280 auto stream_type = rs_get_detached_frame_stream_type(frame, require_no_error());
00281
00282 for (auto& elem : data->number_of_frames_per_stream)
00283 {
00284 if (elem.second < data->duration_per_stream[stream_type].actual_frames_to_receive)
00285 {
00286 stop = false;
00287 break;
00288 }
00289 }
00290
00291
00292 if (stop)
00293 {
00294 stop_streaming = true;
00295 rs_release_frame(dev, frame, require_no_error());
00296 {
00297 std::lock_guard<std::mutex> lk(m);
00298 done = true;
00299 }
00300 cv.notify_one();
00301 return;
00302 }
00303
00304 if (data->duration_per_stream[stream_type].is_end_time_initialized)
00305 {
00306 rs_release_frame(dev, frame, require_no_error());
00307 return;
00308 }
00309
00310 unsigned num_of_frames = 0;
00311 num_of_frames = (++data->number_of_frames_per_stream[stream_type]);
00312
00313 if (num_of_frames >= data->duration_per_stream[stream_type].actual_frames_to_receive)
00314 {
00315 if (!data->duration_per_stream[stream_type].is_end_time_initialized)
00316 {
00317 data->duration_per_stream[stream_type].end_time = std::chrono::high_resolution_clock::now();
00318 data->duration_per_stream[stream_type].is_end_time_initialized = true;
00319 }
00320
00321 rs_release_frame(dev, frame, require_no_error());
00322 return;
00323 }
00324
00325 if ((num_of_frames == data->duration_per_stream[stream_type].first_frame_to_capture) && (!data->duration_per_stream[stream_type].is_start_time_initialized))
00326 {
00327 data->duration_per_stream[stream_type].start_time = std::chrono::high_resolution_clock::now();
00328 data->duration_per_stream[stream_type].is_start_time_initialized = true;
00329 }
00330
00331 REQUIRE( rs_get_detached_frame_data(frame, require_no_error()) != nullptr );
00332 REQUIRE( rs_get_detached_frame_timestamp(frame, require_no_error()) >= 0 );
00333
00334 rs_release_frame(dev, frame, require_no_error());
00335 }
00336
00337 inline void test_frame_callback(rs_device * device, std::initializer_list<stream_mode>& modes, std::map<rs_stream, test_duration>& duration_per_stream)
00338 {
00339 done = false;
00340 stop_streaming = false;
00341 user_data data;
00342 data.duration_per_stream = duration_per_stream;
00343 for(auto & mode : modes)
00344 {
00345 data.number_of_frames_per_stream[mode.stream] = 0;
00346 data.duration_per_stream[mode.stream].is_start_time_initialized = false;
00347 data.duration_per_stream[mode.stream].is_end_time_initialized = false;
00348 data.duration_per_stream[mode.stream].actual_frames_to_receive = std::min(max_frames_to_receive, (uint32_t)(max_capture_time_sec* mode.framerate));
00349 data.duration_per_stream[mode.stream].first_frame_to_capture = (uint32_t)(data.duration_per_stream[mode.stream].actual_frames_to_receive*0.1);
00350 data.duration_per_stream[mode.stream].frames_to_capture = data.duration_per_stream[mode.stream].actual_frames_to_receive - data.duration_per_stream[mode.stream].first_frame_to_capture;
00351 REQUIRE( rs_is_stream_enabled(device, mode.stream, require_no_error()) == 1 );
00352 rs_set_frame_callback(device, mode.stream, frame_callback, &data, require_no_error());
00353 }
00354
00355 rs_start_device(device, require_no_error());
00356 REQUIRE( rs_is_device_streaming(device, require_no_error()) == 1 );
00357
00358 {
00359 std::unique_lock<std::mutex> lk(m);
00360 cv.wait(lk, [&]{return done;});
00361 lk.unlock();
00362 }
00363
00364 rs_stop_device(device, require_no_error());
00365 REQUIRE( rs_is_device_streaming(device, require_no_error()) == 0 );
00366
00367 for(auto & mode : modes)
00368 {
00369 auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(data.duration_per_stream[mode.stream].end_time - data.duration_per_stream[mode.stream].start_time).count();
00370 auto fps = ((float)data.number_of_frames_per_stream[mode.stream] / duration) * 1000.;
00371
00372 }
00373 }
00374
00375 inline void motion_callback(rs_device * , rs_motion_data, void *)
00376 {
00377 }
00378
00379 inline void timestamp_callback(rs_device * , rs_timestamp_data, void *)
00380 {
00381 }
00382
00383
00384 inline void test_streaming(rs_device * device, std::initializer_list<stream_mode> modes)
00385 {
00386 std::map<rs_stream, test_duration> duration_per_stream;
00387 for(auto & mode : modes)
00388 {
00389 duration_per_stream.insert(std::pair<rs_stream, test_duration>(mode.stream, test_duration()));
00390 rs_enable_stream(device, mode.stream, mode.width, mode.height, mode.format, mode.framerate, require_no_error());
00391 REQUIRE( rs_is_stream_enabled(device, mode.stream, require_no_error()) == 1 );
00392 }
00393
00394 test_wait_for_frames(device, modes, duration_per_stream);
00395
00396 test_frame_callback(device, modes, duration_per_stream);
00397
00398 for(auto & mode : modes)
00399 {
00400 rs_disable_stream(device, mode.stream, require_no_error());
00401 REQUIRE( rs_is_stream_enabled(device, mode.stream, require_no_error()) == 0 );
00402 }
00403 }
00404
00405 inline void test_option(rs_device * device, rs_option option, std::initializer_list<int> good_values, std::initializer_list<int> bad_values)
00406 {
00407
00408 const auto first_value = rs_get_device_option(device, option, require_no_error());
00409
00410
00411
00412
00413 for(auto value : good_values)
00414 {
00415 rs_set_device_option(device, option, value, require_no_error());
00416 REQUIRE( rs_get_device_option(device, option, require_no_error()) == value );
00417 }
00418
00419
00420 const auto last_good_value = rs_get_device_option(device, option, require_no_error());
00421 for(auto value : bad_values)
00422 {
00423 rs_set_device_option(device, option, value, require_error("foo",false));
00424 REQUIRE( rs_get_device_option(device, option, require_no_error()) == last_good_value );
00425 }
00426
00427
00428 rs_set_device_option(device, option, first_value, require_no_error());
00429 REQUIRE( rs_get_device_option(device, option, require_no_error()) == first_value );
00430 }
00431 #endif