From 9c4a4fa726a330fe38b601312b20feee2c5608d8 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 3 Jun 2019 11:37:53 +0000 Subject: [PATCH] Add initial realsense support --- CMakeLists.txt | 9 +- include/basalt/io/dataset_io.h | 2 - include/basalt/io/dataset_io_euroc.h | 72 +++- include/basalt/optimization/linearize.h | 2 - include/basalt/utils/image.h | 171 ++++---- include/basalt/utils/keypoints.h | 2 - scripts/verify_dataset.py | 82 ++++ src/calibration/cam_calib.cpp | 5 + src/calibration/vignette.cpp | 14 +- src/opt_flow.cpp | 5 +- src/rs_t265_record.cpp | 541 ++++++++++++++++++++++++ thirdparty/apriltag/CMakeLists.txt | 2 +- 12 files changed, 786 insertions(+), 121 deletions(-) create mode 100755 scripts/verify_dataset.py create mode 100644 src/rs_t265_record.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f4b3cc..11f4730 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -179,7 +179,7 @@ endif() find_package(TBB REQUIRED) include_directories(${TBB_INCLUDE_DIR}) -find_package(OpenCV REQUIRED core imgproc calib3d) +find_package(OpenCV REQUIRED core imgproc calib3d highgui) include_directories(${OpenCV_INCLUDE_DIR}) message(STATUS "Found OpenCV headers in: ${OpenCV_INCLUDE_DIR}") message(STATUS "Found OpenCV_LIBS: ${OpenCV_LIBS}") @@ -244,6 +244,13 @@ target_link_libraries(basalt_opt_flow basalt) add_executable(basalt_vio src/vio.cpp) target_link_libraries(basalt_vio basalt) +find_package(realsense2 QUIET) +if(realsense2_FOUND) + add_executable(basalt_rs_t265_record src/rs_t265_record.cpp) + target_link_libraries(basalt_rs_t265_record basalt realsense2::realsense2 ${OpenCV_LIBS}) +endif() + + install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt EXPORT BasaltTargets diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 9473a55..53b0d5a 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -64,8 +64,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include - namespace basalt { inline bool file_exists(const std::string &name) { diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index d699aaf..8cff4f4 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -40,6 +40,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include namespace fs = std::experimental::filesystem; +#include + namespace basalt { class EurocVioDataset : public VioDataset { @@ -63,6 +65,8 @@ class EurocVioDataset : public VioDataset { int64_t mocap_to_imu_offset_ns; + std::vector> exposure_times; + public: ~EurocVioDataset(){}; @@ -90,27 +94,46 @@ class EurocVioDataset : public VioDataset { path + folder[i] + "data/" + image_path[t_ns]; if (file_exists(full_image_path)) { - pangolin::TypedImage img = pangolin::LoadImage(full_image_path); + cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED); - if (img.fmt.bpp == 8) { - res[i].img.reset(new ManagedImage(img.w, img.h)); + if (img.type() == CV_8UC1) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); - const uint8_t *data_in = img.ptr; + const uint8_t *data_in = img.ptr(); uint16_t *data_out = res[i].img->ptr; - for (size_t i = 0; i < img.size(); i++) { + size_t full_size = img.cols * img.rows; + for (size_t i = 0; i < full_size; i++) { int val = data_in[i]; val = val << 8; data_out[i] = val; } - } else if (img.fmt.bpp == 16) { - res[i].img.reset(new ManagedImage(img.w, img.h)); - std::memcpy(res[i].img->ptr, img.ptr, img.size() * sizeof(uint16_t)); + } else if (img.type() == CV_8UC3) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); + + const uint8_t *data_in = img.ptr(); + uint16_t *data_out = res[i].img->ptr; + + size_t full_size = img.cols * img.rows; + for (size_t i = 0; i < full_size; i++) { + int val = data_in[i * 3]; + val = val << 8; + data_out[i] = val; + } + } else if (img.type() == CV_16UC1) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); + std::memcpy(res[i].img->ptr, img.ptr(), + img.cols * img.rows * sizeof(uint16_t)); } else { - std::cerr << "img.fmt.bpp " << img.fmt.bpp << std::endl; + std::cerr << "img.fmt.bpp " << img.type() << std::endl; std::abort(); } + + auto exp_it = exposure_times[i].find(t_ns); + if (exp_it != exposure_times[i].end()) { + res[i].exposure = exp_it->second; + } } } @@ -144,6 +167,16 @@ class EurocIO : public DatasetIoInterface { } else if (file_exists(path + "/mav0/mocap0/data.csv")) { read_gt_data_pose(path + "/mav0/mocap0/"); } + + data->exposure_times.resize(data->num_cams); + if (file_exists(path + "/mav0/cam0/exposure.csv")) { + std::cout << "Loading exposure times for cam0" << std::endl; + read_exposure(path + "/mav0/cam0/", data->exposure_times[0]); + } + if (file_exists(path + "/mav0/cam1/exposure.csv")) { + std::cout << "Loading exposure times for cam1" << std::endl; + read_exposure(path + "/mav0/cam1/", data->exposure_times[1]); + } } void reset() { data.reset(); } @@ -151,6 +184,27 @@ class EurocIO : public DatasetIoInterface { VioDatasetPtr get_data() { return data; } private: + void read_exposure(const std::string &path, + std::unordered_map &exposure_data) { + exposure_data.clear(); + + std::ifstream f(path + "exposure.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + int64_t timestamp, exposure_int; + Eigen::Vector3d gyro, accel; + + ss >> timestamp >> tmp >> exposure_int; + + exposure_data[timestamp] = exposure_int * 1e-9; + } + } + void read_image_timestamps(const std::string &path) { std::ifstream f(path + "data.csv"); std::string line; diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h index 2927966..6c2a2e7 100644 --- a/include/basalt/optimization/linearize.h +++ b/include/basalt/optimization/linearize.h @@ -40,8 +40,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include - #include namespace basalt { diff --git a/include/basalt/utils/image.h b/include/basalt/utils/image.h index 11c6a1f..2f6c55d 100644 --- a/include/basalt/utils/image.h +++ b/include/basalt/utils/image.h @@ -66,12 +66,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include +#include + +// Renamed Pangoling defines to avoid clash +#define BASALT_HOST_DEVICE +#define BASALT_EXTENSION_IMAGE +#ifdef BASALT_ENABLE_BOUNDS_CHECKS +#define BASALT_BOUNDS_ASSERT(...) BASALT_ASSERT(##__VA_ARGS__) +#else +#define BASALT_BOUNDS_ASSERT(...) ((void)0) +#endif namespace basalt { +template +struct CopyObject { + CopyObject(const T& obj) : obj(obj) {} + const T& obj; +}; + inline void PitchedCopy(char* dst, unsigned int dst_pitch_bytes, const char* src, unsigned int src_pitch_bytes, unsigned int width_bytes, unsigned int height) { @@ -93,42 +106,37 @@ struct Image { inline Image(T* ptr, size_t w, size_t h, size_t pitch) : pitch(pitch), ptr(ptr), w(w), h(h) {} - PANGO_HOST_DEVICE inline size_t SizeBytes() const { return pitch * h; } + BASALT_HOST_DEVICE inline size_t SizeBytes() const { return pitch * h; } - PANGO_HOST_DEVICE inline size_t Area() const { return w * h; } + BASALT_HOST_DEVICE inline size_t Area() const { return w * h; } - PANGO_HOST_DEVICE inline bool IsValid() const { return ptr != 0; } + BASALT_HOST_DEVICE inline bool IsValid() const { return ptr != 0; } - PANGO_HOST_DEVICE inline bool IsContiguous() const { + BASALT_HOST_DEVICE inline bool IsContiguous() const { return w * sizeof(T) == pitch; } - pangolin::Image toPangoImage() { - pangolin::Image img(ptr, w, h, pitch); - return img; - } - ////////////////////////////////////////////////////// // Iterators ////////////////////////////////////////////////////// - PANGO_HOST_DEVICE inline T* begin() { return ptr; } + BASALT_HOST_DEVICE inline T* begin() { return ptr; } - PANGO_HOST_DEVICE inline T* end() { return RowPtr(h - 1) + w; } + BASALT_HOST_DEVICE inline T* end() { return RowPtr(h - 1) + w; } - PANGO_HOST_DEVICE inline const T* begin() const { return ptr; } + BASALT_HOST_DEVICE inline const T* begin() const { return ptr; } - PANGO_HOST_DEVICE inline const T* end() const { return RowPtr(h - 1) + w; } + BASALT_HOST_DEVICE inline const T* end() const { return RowPtr(h - 1) + w; } - PANGO_HOST_DEVICE inline size_t size() const { return w * h; } + BASALT_HOST_DEVICE inline size_t size() const { return w * h; } ////////////////////////////////////////////////////// // Image transforms ////////////////////////////////////////////////////// template - PANGO_HOST_DEVICE inline void Transform(UnaryOperation unary_op) { - PANGO_ASSERT(IsValid()); + BASALT_HOST_DEVICE inline void Transform(UnaryOperation unary_op) { + BASALT_ASSERT(IsValid()); for (size_t y = 0; y < h; ++y) { T* el = RowPtr(y); @@ -139,32 +147,32 @@ struct Image { } } - PANGO_HOST_DEVICE inline void Fill(const T& val) { + BASALT_HOST_DEVICE inline void Fill(const T& val) { Transform([&](const T&) { return val; }); } - PANGO_HOST_DEVICE inline void Replace(const T& oldval, const T& newval) { + BASALT_HOST_DEVICE inline void Replace(const T& oldval, const T& newval) { Transform([&](const T& val) { return (val == oldval) ? newval : val; }); } inline void Memset(unsigned char v = 0) { - PANGO_ASSERT(IsValid()); + BASALT_ASSERT(IsValid()); if (IsContiguous()) { - ::pangolin::Memset((char*)ptr, v, pitch * h); + std::memset((char*)ptr, v, pitch * h); } else { for (size_t y = 0; y < h; ++y) { - ::pangolin::Memset((char*)RowPtr(y), v, pitch); + std::memset((char*)RowPtr(y), v, pitch); } } } inline void CopyFrom(const Image& img) { if (IsValid() && img.IsValid()) { - PANGO_ASSERT(w >= img.w && h >= img.h); + BASALT_ASSERT(w >= img.w && h >= img.h); PitchedCopy((char*)ptr, pitch, (char*)img.ptr, img.pitch, std::min(img.w, w) * sizeof(T), std::min(img.h, h)); } else if (img.IsValid() != IsValid()) { - PANGO_ASSERT(false && "Cannot copy from / to an unasigned image."); + BASALT_ASSERT(false && "Cannot copy from / to an unasigned image."); } } @@ -173,9 +181,9 @@ struct Image { ////////////////////////////////////////////////////// template - PANGO_HOST_DEVICE inline T Accumulate(const T init, - BinaryOperation binary_op) { - PANGO_ASSERT(IsValid()); + BASALT_HOST_DEVICE inline T Accumulate(const T init, + BinaryOperation binary_op) { + BASALT_ASSERT(IsValid()); T val = init; for (size_t y = 0; y < h; ++y) { @@ -189,7 +197,7 @@ struct Image { } std::pair MinMax() const { - PANGO_ASSERT(IsValid()); + BASALT_ASSERT(IsValid()); std::pair minmax(std::numeric_limits::max(), std::numeric_limits::lowest()); @@ -220,43 +228,43 @@ struct Image { // Direct Pixel Access ////////////////////////////////////////////////////// - PANGO_HOST_DEVICE inline T* RowPtr(size_t y) { + BASALT_HOST_DEVICE inline T* RowPtr(size_t y) { return (T*)((unsigned char*)(ptr) + y * pitch); } - PANGO_HOST_DEVICE inline const T* RowPtr(size_t y) const { + BASALT_HOST_DEVICE inline const T* RowPtr(size_t y) const { return (T*)((unsigned char*)(ptr) + y * pitch); } - PANGO_HOST_DEVICE inline T& operator()(size_t x, size_t y) { - PANGO_BOUNDS_ASSERT(InBounds(x, y)); + BASALT_HOST_DEVICE inline T& operator()(size_t x, size_t y) { + BASALT_BOUNDS_ASSERT(InBounds(x, y)); return RowPtr(y)[x]; } - PANGO_HOST_DEVICE inline const T& operator()(size_t x, size_t y) const { - PANGO_BOUNDS_ASSERT(InBounds(x, y)); + BASALT_HOST_DEVICE inline const T& operator()(size_t x, size_t y) const { + BASALT_BOUNDS_ASSERT(InBounds(x, y)); return RowPtr(y)[x]; } template - PANGO_HOST_DEVICE inline T& operator()(const TVec& p) { - PANGO_BOUNDS_ASSERT(InBounds(p[0], p[1])); + BASALT_HOST_DEVICE inline T& operator()(const TVec& p) { + BASALT_BOUNDS_ASSERT(InBounds(p[0], p[1])); return RowPtr(p[1])[p[0]]; } template - PANGO_HOST_DEVICE inline const T& operator()(const TVec& p) const { - PANGO_BOUNDS_ASSERT(InBounds(p[0], p[1])); + BASALT_HOST_DEVICE inline const T& operator()(const TVec& p) const { + BASALT_BOUNDS_ASSERT(InBounds(p[0], p[1])); return RowPtr(p[1])[p[0]]; } - PANGO_HOST_DEVICE inline T& operator[](size_t ix) { - PANGO_BOUNDS_ASSERT(InImage(ptr + ix)); + BASALT_HOST_DEVICE inline T& operator[](size_t ix) { + BASALT_BOUNDS_ASSERT(InImage(ptr + ix)); return ptr[ix]; } - PANGO_HOST_DEVICE inline const T& operator[](size_t ix) const { - PANGO_BOUNDS_ASSERT(InImage(ptr + ix)); + BASALT_HOST_DEVICE inline const T& operator[](size_t ix) const { + BASALT_BOUNDS_ASSERT(InImage(ptr + ix)); return ptr[ix]; } @@ -346,21 +354,22 @@ struct Image { // Bounds Checking ////////////////////////////////////////////////////// - PANGO_HOST_DEVICE + BASALT_HOST_DEVICE bool InImage(const T* ptest) const { return ptr <= ptest && ptest < RowPtr(h); } - PANGO_HOST_DEVICE inline bool InBounds(int x, int y) const { + BASALT_HOST_DEVICE inline bool InBounds(int x, int y) const { return 0 <= x && x < (int)w && 0 <= y && y < (int)h; } - PANGO_HOST_DEVICE inline bool InBounds(float x, float y, float border) const { + BASALT_HOST_DEVICE inline bool InBounds(float x, float y, + float border) const { return border <= x && x < (w - border) && border <= y && y < (h - border); } template - PANGO_HOST_DEVICE inline bool InBounds( + BASALT_HOST_DEVICE inline bool InBounds( const TVec& p, const TBorder border = (TBorder)0) const { return border <= p[0] && p[0] < ((int)w - border) && border <= p[1] && p[1] < ((int)h - border); @@ -370,24 +379,24 @@ struct Image { // Obtain slices / subimages ////////////////////////////////////////////////////// - PANGO_HOST_DEVICE inline const Image SubImage(size_t x, size_t y, - size_t width, - size_t height) const { - PANGO_ASSERT((x + width) <= w && (y + height) <= h); + BASALT_HOST_DEVICE inline const Image SubImage(size_t x, size_t y, + size_t width, + size_t height) const { + BASALT_ASSERT((x + width) <= w && (y + height) <= h); return Image(RowPtr(y) + x, width, height, pitch); } - PANGO_HOST_DEVICE inline Image SubImage(size_t x, size_t y, size_t width, - size_t height) { - PANGO_ASSERT((x + width) <= w && (y + height) <= h); + BASALT_HOST_DEVICE inline Image SubImage(size_t x, size_t y, size_t width, + size_t height) { + BASALT_ASSERT((x + width) <= w && (y + height) <= h); return Image(RowPtr(y) + x, width, height, pitch); } - PANGO_HOST_DEVICE inline Image Row(int y) const { + BASALT_HOST_DEVICE inline Image Row(int y) const { return SubImage(0, y, w, 1); } - PANGO_HOST_DEVICE inline Image Col(int x) const { + BASALT_HOST_DEVICE inline Image Col(int x) const { return SubImage(x, 0, 1, h); } @@ -396,15 +405,15 @@ struct Image { ////////////////////////////////////////////////////// template - PANGO_HOST_DEVICE inline Image Reinterpret() const { - PANGO_ASSERT(sizeof(TRecast) == sizeof(T), - "sizeof(TRecast) must match sizeof(T): % != %", - sizeof(TRecast), sizeof(T)); + BASALT_HOST_DEVICE inline Image Reinterpret() const { + BASALT_ASSERT_STREAM(sizeof(TRecast) == sizeof(T), + "sizeof(TRecast) must match sizeof(T): " + << sizeof(TRecast) << " != " << sizeof(T)); return UnsafeReinterpret(); } template - PANGO_HOST_DEVICE inline Image UnsafeReinterpret() const { + BASALT_HOST_DEVICE inline Image UnsafeReinterpret() const { return Image((TRecast*)ptr, w, h, pitch); } @@ -444,7 +453,7 @@ struct Image { size_t w; size_t h; - PANGO_EXTENSION_IMAGE + BASALT_EXTENSION_IMAGE }; template @@ -491,30 +500,15 @@ class ManagedImage : public Image { img.ptr = nullptr; } - // Move constructor - inline ManagedImage(pangolin::ManagedImage&& img) { - *this = std::move(img); - } - - // Move asignment - inline void operator=(pangolin::ManagedImage&& img) { - Deallocate(); - Image::pitch = img.pitch; - Image::ptr = img.ptr; - Image::w = img.w; - Image::h = img.h; - img.ptr = nullptr; - } - // Explicit copy constructor template - ManagedImage(const pangolin::CopyObject& other) { + ManagedImage(const CopyObject& other) { CopyFrom(other.obj); } // Explicit copy assignment template - void operator=(const pangolin::CopyObject& other) { + void operator=(const CopyObject& other) { CopyFrom(other.obj); } @@ -677,8 +671,6 @@ class ManagedImagePyr { return Eigen::Matrix(x, y); } - inline pangolin::Image toPangoImage() { return image.toPangoImage(); } - private: inline Image lvl_internal(size_t lvl) { size_t x = (lvl == 0) ? 0 : orig_w; @@ -693,19 +685,4 @@ class ManagedImagePyr { ManagedImage image; }; -inline void rgb_to_gray(const pangolin::TypedImage& rgb, - basalt::ManagedImage& gray) { - gray.Reinitialise(rgb.w, rgb.h); - - for (size_t x = 0; x < rgb.w; x++) { - for (size_t y = 0; y < rgb.h; y++) { - double val = 0.2989 * (double)rgb(3 * x + 0, y) + - 0.5870 * (double)rgb(3 * x + 1, y) + - 0.1140 * (double)rgb(3 * x + 2, y); - - gray(x, y) = val; - } - } -} - } // namespace basalt diff --git a/include/basalt/utils/keypoints.h b/include/basalt/utils/keypoints.h index 3cd3768..d37dd5a 100644 --- a/include/basalt/utils/keypoints.h +++ b/include/basalt/utils/keypoints.h @@ -40,8 +40,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include - #include #include diff --git a/scripts/verify_dataset.py b/scripts/verify_dataset.py new file mode 100755 index 0000000..60d34d5 --- /dev/null +++ b/scripts/verify_dataset.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python + +import sys +import math +import os + +import numpy as np + +dataset_path = sys.argv[1] + +print dataset_path + +timestamps = {} +exposures = {} + + +for sensor in ['cam0', 'cam1', 'imu0']: + data = np.loadtxt(dataset_path + '/mav0/' + sensor + '/data.csv', usecols=[0], delimiter=',', dtype=np.int64) + timestamps[sensor] = data + +# check if dataset is OK... +for key, value in timestamps.iteritems(): + times = value * 1e-9 + min_t = times.min() + max_t = times.max() + interval = max_t - min_t + diff = times[1:] - times[:-1] + print '==========================================' + print 'sensor', key + print 'min timestamp', min_t + print 'max timestamp', max_t + print 'interval', interval + print 'hz', times.shape[0]/interval + print 'min time between consecutive msgs', diff.min() + print 'max time between consecutive msgs', diff.max() + for i, d in enumerate(diff): + # Note: 0.001 is just a hacky heuristic, since we have nothing faster than 1000Hz. Should maybe be topic-specific. + if d < 0.001: + print("ERROR: Difference on consecutive measurements too small: {} - {} = {}".format(times[i+1], times[i], d)) + +# check if we have all images for timestamps +timestamp_to_topic = {} + +for key, value in timestamps.iteritems(): + if not key.startswith('cam'): continue + for v in value: + if v not in timestamp_to_topic: + timestamp_to_topic[v] = list() + timestamp_to_topic[v].append(key) + +for key in timestamp_to_topic.keys(): + if len(timestamp_to_topic[key]) != 2: + print 'timestamp', key, 'has topics', timestamp_to_topic[key] + + +# check image data. +img_extensions = ['.png', '.jpg', '.webp'] +for key, value in timestamps.iteritems(): + if not key.startswith('cam'): continue + for v in value: + path = dataset_path + '/mav0/' + key + '/data/' + str(v) + img_exists = False + for e in img_extensions: + if os.path.exists(dataset_path + '/mav0/' + key + '/data/' + str(v) + e): + img_exists = True + + if not img_exists: + print('No image data for ' + key + ' at timestamp ' + str(v)) + + exposure_file = dataset_path + '/mav0/' + key + '/exposure.csv' + if not os.path.exists(exposure_file): + print('No exposure data for ' + key) + continue + + exposure_data = np.loadtxt(exposure_file, delimiter=',', dtype=np.int64) + for v in value: + idx = np.searchsorted(exposure_data[:, 0], v) + if exposure_data[idx, 0] != v: + print('No exposure data for ' + key + ' at timestamp ' + str(v)) + + + diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index baa0141..24bad74 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -180,6 +180,11 @@ void CamCalib::computeVign() { it->second.corners_proj_success[k]) { double val = img_vec[j].img->interp(pos); val /= std::numeric_limits::max(); + + if (img_vec[j].exposure > 0) { + val *= 0.001 / img_vec[j].exposure; // bring to common exposure + } + rv[k][2] = val; } else { rv[k][2] = -1; diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp index 78f47f1..75f860e 100644 --- a/src/calibration/vignette.cpp +++ b/src/calibration/vignette.cpp @@ -35,6 +35,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + namespace basalt { VignetteEstimator::VignetteEstimator( @@ -280,8 +282,7 @@ void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) { void VignetteEstimator::save_vign_png(const std::string &path) { for (size_t k = 0; k < vio_dataset->get_num_cams(); k++) { - pangolin::ManagedImage vign_img(resolutions[k][0], - resolutions[k][1]); + ManagedImage vign_img(resolutions[k][0], resolutions[k][1]); vign_img.Fill(0); Eigen::Vector2d oc = optical_centers[k]; @@ -298,9 +299,12 @@ void VignetteEstimator::save_vign_png(const std::string &path) { } } - pangolin::SaveImage(vign_img.UnsafeReinterpret(), - pangolin::PixelFormatFromString("GRAY16LE"), - path + "/vingette_" + std::to_string(k) + ".png"); + // pangolin::SaveImage(vign_img.UnsafeReinterpret(), + // pangolin::PixelFormatFromString("GRAY16LE"), + // path + "/vingette_" + std::to_string(k) + ".png"); + + cv::Mat img(vign_img.h, vign_img.w, CV_16U, vign_img.ptr); + cv::imwrite(path + "/vingette_" + std::to_string(k) + ".png", img); } } } // namespace basalt diff --git a/src/opt_flow.cpp b/src/opt_flow.cpp index ab74b3e..36395c7 100644 --- a/src/opt_flow.cpp +++ b/src/opt_flow.cpp @@ -244,14 +244,15 @@ int main(int argc, char** argv) { for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) { if (img_vec[cam_id].img.get()) { - auto img = img_vec[cam_id].img->toPangoImage(); + auto img = img_vec[cam_id].img; pangolin::GlPixFormat fmt; fmt.glformat = GL_LUMINANCE; fmt.gltype = GL_UNSIGNED_SHORT; fmt.scalable_internal_format = GL_LUMINANCE16; - img_view[cam_id]->SetImage(img.ptr, img.w, img.h, img.pitch, fmt); + img_view[cam_id]->SetImage(img->ptr, img->w, img->h, img->pitch, + fmt); } else { img_view[cam_id]->Clear(); } diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp new file mode 100644 index 0000000..4e26668 --- /dev/null +++ b/src/rs_t265_record.cpp @@ -0,0 +1,541 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +namespace fs = std::experimental::filesystem; + +constexpr int NUM_CAMS = 2; +constexpr int UI_WIDTH = 200; + +pangolin::DataLog imu_log; + +pangolin::Var webp_quality("ui.webp_quality", 90, 0, 101); +pangolin::Var skip_frames("ui.skip_frames", 1, 1, 10); + +struct ImageData { + using Ptr = std::shared_ptr; + + int cam_id; + double exposure_time; + int64_t timestamp; + cv::Mat image; +}; + +struct RsIMUData { + double timestamp; + Eigen::Vector3d data; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +ImageData::Ptr last_images[NUM_CAMS]; +tbb::concurrent_bounded_queue image_save_queue; +float exposure; +std::string dataset_dir; +std::string dataset_folder; +std::string result_dir; + +std::atomic stop_workers; +std::atomic record; + +std::ofstream cam_data[NUM_CAMS], exposure_data[NUM_CAMS], imu0_data; + +std::string get_date(); + +void image_save_worker() { + ImageData::Ptr img; + + while (!stop_workers) { + if (image_save_queue.try_pop(img)) { + std::string filename = dataset_folder + "mav0/cam" + + std::to_string(img->cam_id) + "/data/" + + std::to_string(img->timestamp) + ".webp"; + + std::vector compression_params = {cv::IMWRITE_WEBP_QUALITY, + webp_quality}; + + cv::imwrite(filename, img->image, compression_params); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } +} + +void toggle_recording() { + record = !record; + if (record) { + dataset_folder = dataset_dir + "dataset_" + get_date() + "/"; + fs::create_directory(dataset_folder); + fs::create_directory(dataset_folder + "mav0/"); + fs::create_directory(dataset_folder + "mav0/cam0/"); + fs::create_directory(dataset_folder + "mav0/cam0/data/"); + fs::create_directory(dataset_folder + "mav0/cam1/"); + fs::create_directory(dataset_folder + "mav0/cam1/data/"); + fs::create_directory(dataset_folder + "mav0/imu0/"); + + cam_data[0].open(dataset_folder + "mav0/cam0/data.csv"); + cam_data[1].open(dataset_folder + "mav0/cam1/data.csv"); + exposure_data[0].open(dataset_folder + "mav0/cam0/exposure.csv"); + exposure_data[1].open(dataset_folder + "mav0/cam1/exposure.csv"); + imu0_data.open(dataset_folder + "mav0/imu0/data.csv"); + + cam_data[0] << "#timestamp [ns], filename\n"; + cam_data[1] << "#timestamp [ns], filename\n"; + exposure_data[0] << "#timestamp [ns], exposure time[ns]\n"; + exposure_data[1] << "#timestamp [ns], exposure time[ns]\n"; + imu0_data << "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad " + "s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y " + "[m s^-2],a_RS_S_z [m s^-2]\n"; + + std::cout << "Started recording dataset in " << dataset_folder << std::endl; + + } else { + cam_data[0].close(); + cam_data[1].close(); + exposure_data[0].close(); + exposure_data[1].close(); + imu0_data.close(); + + std::cout << "Stopped recording dataset in " << dataset_folder << std::endl; + } +} + +void export_device_calibration(rs2::pipeline_profile &profile, + const std::string &out_path) { + using Scalar = double; + + std::shared_ptr> calib; + calib.reset(new basalt::Calibration); + + auto accel_stream = profile.get_stream(RS2_STREAM_ACCEL); + auto gyro_stream = profile.get_stream(RS2_STREAM_GYRO); + auto cam0_stream = profile.get_stream(RS2_STREAM_FISHEYE, 1); + auto cam1_stream = profile.get_stream(RS2_STREAM_FISHEYE, 2); + + // get gyro extrinsics + if (auto gyro = gyro_stream.as()) { + // TODO: gyro + rs2_motion_device_intrinsic intrinsics = gyro.get_motion_intrinsics(); + + std::cout << " Scale X cross axis cross axis Bias X \n"; + std::cout << " cross axis Scale Y cross axis Bias Y \n"; + std::cout << " cross axis cross axis Scale Z Bias Z \n"; + for (auto &i : intrinsics.data) { + for (int j = 0; j < 4; j++) { + std::cout << i[j] << " "; + } + std::cout << "\n"; + } + + std::cout << "Variance of noise for X, Y, Z axis \n"; + for (float noise_variance : intrinsics.noise_variances) + std::cout << noise_variance << " "; + std::cout << "\n"; + + std::cout << "Variance of bias for X, Y, Z axis \n"; + for (float bias_variance : intrinsics.bias_variances) + std::cout << bias_variance << " "; + std::cout << "\n"; + } else { + throw std::exception(); + } + + // get accel extrinsics + if (auto gyro = accel_stream.as()) { + // TODO: accel + // rs2_motion_device_intrinsic intrinsics = accel.get_motion_intrinsics(); + } else { + throw std::exception(); + } + + // get camera ex-/intrinsics + for (const auto &cam_stream : {cam0_stream, cam1_stream}) { + if (auto cam = cam_stream.as()) { + // extrinsics + rs2_extrinsics ex = cam.get_extrinsics_to(gyro_stream); + Eigen::Matrix3f rot = Eigen::Map(ex.rotation); + Eigen::Vector3f trans = Eigen::Map(ex.translation); + + Eigen::Quaterniond q(rot.cast()); + basalt::Calibration::SE3 T_i_c(q, trans.cast()); + + std::cout << "T_i_c\n" << T_i_c.matrix() << std::endl; + + calib->T_i_c.push_back(T_i_c); + + // get resolution + Eigen::Vector2i resolution; + resolution << cam.width(), cam.height(); + calib->resolution.push_back(resolution); + + // intrinsics + rs2_intrinsics intrinsics = cam.get_intrinsics(); + basalt::KannalaBrandtCamera4::VecN params; + params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy, + intrinsics.coeffs[0], intrinsics.coeffs[1], intrinsics.coeffs[2], + intrinsics.coeffs[3]; + + std::cout << "params: " << params.transpose() << std::endl; + + basalt::GenericCamera camera; + basalt::KannalaBrandtCamera4 kannala_brandt(params); + camera.variant = kannala_brandt; + + calib->intrinsics.push_back(camera); + } else { + throw std::exception(); // TODO: better exception + } + } + + // serialize and store calibration + // std::ofstream os(out_path + "calibration.json"); + // cereal::JSONOutputArchive archive(os); + + // archive(*calib); +} + +int main(int argc, char *argv[]) { + CLI::App app{"Record RealSense T265 Data"}; + + app.add_option("--dataset-dir", dataset_dir, "Path to dataset"); + app.add_option("--exposure", exposure, + "If set will enable manual exposure, value in microseconds."); + app.add_option("--result-dir", result_dir, + "If set will enable manual exposure, value in microseconds."); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError &e) { + return app.exit(e); + } + + bool show_gui = true; + + image_save_queue.set_capacity(5000); + + stop_workers = false; + std::vector worker_threads; + for (int i = 0; i < 8; i++) { + worker_threads.emplace_back(image_save_worker); + } + + std::string color_mode; + + // realsense + rs2::log_to_console(RS2_LOG_SEVERITY_ERROR); + rs2::context ctx; + rs2::pipeline pipe(ctx); + rs2::config cfg; + + // Add streams of gyro and accelerometer to configuration + cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); + cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); + cfg.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8); + cfg.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8); + + // Using the device_hub we can block the program until a device connects + // rs2::device_hub device_hub(ctx); + + auto devices = ctx.query_devices(); + if (devices.size() == 0) { + std::abort(); + } + auto device = devices[0]; + std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) + << " connected" << std::endl; + auto sens = device.query_sensors()[0]; + + if (exposure > 0) { + std::cout << "Setting exposure to: " << exposure << " microseconds" + << std::endl; + sens.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); + sens.set_option(rs2_option::RS2_OPTION_EXPOSURE, (float)exposure); + } + + std::mutex data_mutex; + + rs2::motion_frame last_gyro_meas = rs2::motion_frame(rs2::frame()); + Eigen::deque gyro_data_queue; + + std::shared_ptr prev_accel_data; + + int processed_frame = 0; + + auto callback = [&](const rs2::frame &frame) { + std::lock_guard lock(data_mutex); + + if (auto fp = frame.as()) { + auto motion = frame.as(); + + if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO && + motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) { + RsIMUData d; + d.timestamp = motion.get_timestamp(); + d.data << motion.get_motion_data().x, motion.get_motion_data().y, + motion.get_motion_data().z; + + gyro_data_queue.emplace_back(d); + } + + if (motion && motion.get_profile().stream_type() == RS2_STREAM_ACCEL && + motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) { + RsIMUData d; + d.timestamp = motion.get_timestamp(); + d.data << motion.get_motion_data().x, motion.get_motion_data().y, + motion.get_motion_data().z; + + if (!prev_accel_data.get()) { + prev_accel_data.reset(new RsIMUData(d)); + } else { + BASALT_ASSERT(d.timestamp > prev_accel_data->timestamp); + + while (!gyro_data_queue.empty() && gyro_data_queue.front().timestamp < + prev_accel_data->timestamp) { + std::cout << "Skipping gyro data. Timestamp before the first accel " + "measurement."; + gyro_data_queue.pop_front(); + } + + while (!gyro_data_queue.empty() && + gyro_data_queue.front().timestamp < d.timestamp) { + RsIMUData gyro_data = gyro_data_queue.front(); + gyro_data_queue.pop_front(); + + double w0 = (d.timestamp - gyro_data.timestamp) / + (d.timestamp - prev_accel_data->timestamp); + + double w1 = (gyro_data.timestamp - prev_accel_data->timestamp) / + (d.timestamp - prev_accel_data->timestamp); + + Eigen::Vector3d accel_interpolated = + w0 * prev_accel_data->data + w1 * d.data; + + if (record) { + int64_t timestamp = gyro_data.timestamp * 1e6; + imu0_data << timestamp << "," << gyro_data.data[0] << "," + << gyro_data.data[1] << "," << gyro_data.data[2] << "," + << accel_interpolated[0] << "," << accel_interpolated[1] + << "," << accel_interpolated[2] << "\n"; + } + + imu_log.Log(accel_interpolated[0], accel_interpolated[1], + accel_interpolated[2]); + } + + prev_accel_data.reset(new RsIMUData(d)); + } + } + } + + if (auto fs = frame.as()) { + processed_frame++; + if (processed_frame % int(skip_frames) != 0) return; + + for (int i = 0; i < NUM_CAMS; i++) { + auto f = fs[i]; + if (!f.as()) { + std::cout << "Weird Frame, skipping" << std::endl; + continue; + } + auto vf = f.as(); + + last_images[i].reset(new ImageData); + last_images[i]->image = cv::Mat(vf.get_height(), vf.get_width(), CV_8U); + std::memcpy( + last_images[i]->image.ptr(), vf.get_data(), + vf.get_width() * vf.get_height() * vf.get_bytes_per_pixel()); + + last_images[i]->exposure_time = + vf.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE); + + last_images[i]->timestamp = vf.get_timestamp() * 1e6; + last_images[i]->cam_id = i; + + if (record) { + image_save_queue.push(last_images[i]); + + cam_data[i] << last_images[i]->timestamp << "," + << last_images[i]->timestamp << ".webp" << std::endl; + + exposure_data[i] << last_images[i]->timestamp << "," + << int64_t(vf.get_frame_metadata( + RS2_FRAME_METADATA_ACTUAL_EXPOSURE) * + 1e3) + << std::endl; + } + } + } + }; + + // Start streaming through the callback + rs2::pipeline_profile profiles = pipe.start(cfg, callback); + + { + auto sensors = profiles.get_device().query_sensors(); + + for (auto &s : sensors) { + std::cout << "Sensor " << s.get_info(RS2_CAMERA_INFO_NAME) + << ". Supported options:" << std::endl; + + for (const auto &o : s.get_supported_options()) { + std::cout << "\t" << rs2_option_to_string(o) << std::endl; + } + } + } + + export_device_calibration(profiles, result_dir); + + if (show_gui) { + pangolin::CreateWindowAndBind("Record RealSense T265", 1200, 800); + + pangolin::Var> record_btn("ui.record", + toggle_recording); + + std::atomic record_t_ns; + record_t_ns = 0; + + glEnable(GL_DEPTH_TEST); + + pangolin::View &img_view_display = + pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < NUM_CAMS) { + int idx = img_view.size(); + std::shared_ptr iv(new pangolin::ImageView); + + iv->extern_draw_function = [&, idx](pangolin::View &v) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); // red + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (last_images[idx].get()) + pangolin::GlFont::I() + .Text("Exposure: %.3f ms.", + last_images[idx]->exposure_time / 1000.0) + .Draw(30, 30); + + if (idx == 0) { + pangolin::GlFont::I() + .Text("Queue: %d.", image_save_queue.size()) + .Draw(30, 60); + } + + if (idx == 0 && record) { + pangolin::GlFont::I().Text("Recording").Draw(30, 90); + } + }; + + iv->OnSelectionCallback = + [&](pangolin::ImageView::OnSelectionEventData o) { + int64_t curr_t_ns = std::chrono::high_resolution_clock::now() + .time_since_epoch() + .count(); + if (std::abs(record_t_ns - curr_t_ns) > int64_t(2e9)) { + toggle_recording(); + record_t_ns = curr_t_ns; + } + }; + + img_view.push_back(iv); + img_view_display.AddDisplay(*iv); + } + + imu_log.Clear(); + + std::vector labels; + labels.push_back(std::string("accel x")); + labels.push_back(std::string("accel y")); + labels.push_back(std::string("accel z")); + imu_log.SetLabels(labels); + + pangolin::Plotter plotter(&imu_log, 0.0f, 2000.0f, -15.0f, 15.0f, 0.1f, + 0.1f); + plotter.SetBounds(0.0, 1.0, 0.0, 1.0); + plotter.Track("$i"); + + plot_display.AddDisplay(plotter); + + plotter.ClearSeries(); + plotter.AddSeries("$i", "$0", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "accel x"); + plotter.AddSeries("$i", "$1", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "accel y"); + plotter.AddSeries("$i", "$2", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "accel z"); + + while (!pangolin::ShouldQuit()) { + { + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_BYTE; + fmt.scalable_internal_format = GL_LUMINANCE8; + + for (size_t cam_id = 0; cam_id < NUM_CAMS; cam_id++) { + if (last_images[cam_id].get()) + img_view[cam_id]->SetImage(last_images[cam_id]->image.ptr(), + last_images[cam_id]->image.cols, + last_images[cam_id]->image.rows, + last_images[cam_id]->image.step, fmt); + } + } + + pangolin::FinishFrame(); + + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } + + stop_workers = true; + for (auto &t : worker_threads) { + t.join(); + } + + return EXIT_SUCCESS; +} + +std::string get_date() { + constexpr int MAX_DATE = 64; + time_t now; + char the_date[MAX_DATE]; + + the_date[0] = '\0'; + + now = time(NULL); + + if (now != -1) { + strftime(the_date, MAX_DATE, "%Y_%m_%d_%H_%M_%S", gmtime(&now)); + } + + return std::string(the_date); +} diff --git a/thirdparty/apriltag/CMakeLists.txt b/thirdparty/apriltag/CMakeLists.txt index 0c1a28e..c9a7a78 100644 --- a/thirdparty/apriltag/CMakeLists.txt +++ b/thirdparty/apriltag/CMakeLists.txt @@ -20,6 +20,6 @@ include_directories(../basalt-headers/thirdparty/Sophus) add_library(apriltag STATIC ${APRILTAG_SRCS} src/apriltag.cpp) target_include_directories(apriltag PUBLIC include) -target_link_libraries(apriltag PUBLIC ${OpenCV_LIBS} pangolin) +target_link_libraries(apriltag PUBLIC ${OpenCV_LIBS})