diff --git a/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp b/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp index f601b98..f2ece51 100644 --- a/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp +++ b/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp @@ -1,72 +1,74 @@ #pragma once -#include #include +#include #include namespace event_camera_simulator { -class ImageBuffer -{ -public: - ZE_POINTER_TYPEDEFS(ImageBuffer); + class ImageBuffer { + public: + ZE_POINTER_TYPEDEFS(ImageBuffer); - struct ImageData - { - ImageData(Image img, Time stamp, Duration exposure_time) - : image(img), - stamp(stamp), - exposure_time(exposure_time) {} + struct ImageData { + ImageData(Image img, Time stamp, Duration exposure_time) + : image(img), + stamp(stamp), + exposure_time(exposure_time) {} - Image image; - Time stamp; - Duration exposure_time; // timestamp since last image (0 if this is the first image) - }; + Image image; + Time stamp; + Duration exposure_time; // timestamp since last image (0 if this is + // the first image) + }; - using ExposureImage = std::pair; + using ExposureImage = std::pair; - // Rolling image buffer of mazimum size 'buffer_size_ns'. - ImageBuffer(Duration buffer_size_ns) - : buffer_size_ns_(buffer_size_ns) {} + // Rolling image buffer of mazimum size 'buffer_size_ns'. + ImageBuffer(Duration buffer_size_ns): buffer_size_ns_(buffer_size_ns) {} - void addImage(Time t, const Image& img); + void addImage(Time t, const Image& img); - std::deque getRawBuffer() const { return data_; } + std::deque getRawBuffer() const { + return data_; + } - size_t size() const { return data_.size(); } + size_t size() const { + return data_.size(); + } - Duration getExposureTime() const { return buffer_size_ns_; } + Duration getExposureTime() const { + return buffer_size_ns_; + } -private: - Duration buffer_size_ns_; - std::deque data_; -}; + private: + Duration buffer_size_ns_; + std::deque data_; + }; + /* + * The CameraSimulator takes as input a sequence of stamped images, + * assumed to be sampled at a "sufficiently high" framerate and with + * floating-point precision, and treats each image as a measure of + * irradiance. + * From this, it simulates a real camera, including motion blur. + * + * @TODO: simulate a non-linear camera response curve, shot noise, etc. + */ + class CameraSimulator { + public: + CameraSimulator(double exposure_time_ms) + : exposure_time_(ze::secToNanosec(exposure_time_ms / 1000.0)) { + buffer_.reset(new ImageBuffer(exposure_time_)); + } -/* - * The CameraSimulator takes as input a sequence of stamped images, - * assumed to be sampled at a "sufficiently high" framerate and with - * floating-point precision, and treats each image as a measure of - * irradiance. - * From this, it simulates a real camera, including motion blur. - * - * @TODO: simulate a non-linear camera response curve, shot noise, etc. - */ -class CameraSimulator -{ -public: - CameraSimulator(double exposure_time_ms) - : exposure_time_(ze::secToNanosec(exposure_time_ms / 1000.0)) - { - buffer_.reset(new ImageBuffer(exposure_time_)); - } + bool imageCallback( + const Image& img, Time time, const ImagePtr& camera_image + ); - bool imageCallback(const Image& img, Time time, - const ImagePtr &camera_image); - -private: - ImageBuffer::Ptr buffer_; - const Duration exposure_time_; -}; + private: + ImageBuffer::Ptr buffer_; + const Duration exposure_time_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp b/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp index fc8ad03..df34236 100644 --- a/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp +++ b/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp @@ -4,51 +4,47 @@ namespace event_camera_simulator { -/* - * The EventSimulator takes as input a sequence of stamped images, - * assumed to be sampled at a "sufficiently high" framerate, - * and simulates the principle of operation of an idea event camera - * with a constant contrast threshold C. - * Pixel-wise intensity values are linearly interpolated in time. - * - * The pixel-wise voltages are reset with the values from the first image - * which is passed to the simulator. - */ -class EventSimulator -{ -public: + /* + * The EventSimulator takes as input a sequence of stamped images, + * assumed to be sampled at a "sufficiently high" framerate, + * and simulates the principle of operation of an idea event camera + * with a constant contrast threshold C. + * Pixel-wise intensity values are linearly interpolated in time. + * + * The pixel-wise voltages are reset with the values from the first image + * which is passed to the simulator. + */ + class EventSimulator { + public: + struct Config { + double Cp; + double Cm; + double sigma_Cp; + double sigma_Cm; + Duration refractory_period_ns; + bool use_log_image; + double log_eps; + }; - struct Config - { - double Cp; - double Cm; - double sigma_Cp; - double sigma_Cm; - Duration refractory_period_ns; - bool use_log_image; - double log_eps; - }; + using TimestampImage = cv::Mat_; - using TimestampImage = cv::Mat_; + EventSimulator(const Config& config) + : config_(config), + is_initialized_(false), + current_time_(0) {} - EventSimulator(const Config& config) - : config_(config), - is_initialized_(false), - current_time_(0) - {} + void init(const Image& img, Time time); + Events imageCallback(const Image& img, Time time); - void init(const Image& img, Time time); - Events imageCallback(const Image& img, Time time); + private: + bool is_initialized_; + Time current_time_; + Image ref_values_; + Image last_img_; + TimestampImage last_event_timestamp_; + cv::Size size_; -private: - bool is_initialized_; - Time current_time_; - Image ref_values_; - Image last_img_; - TimestampImage last_event_timestamp_; - cv::Size size_; - - Config config_; -}; + Config config_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim/include/esim/esim/simulator.hpp b/event_camera_simulator/esim/include/esim/esim/simulator.hpp index f36b1d4..8b31290 100644 --- a/event_camera_simulator/esim/include/esim/esim/simulator.hpp +++ b/event_camera_simulator/esim/include/esim/esim/simulator.hpp @@ -1,60 +1,60 @@ #pragma once -#include #include +#include #include namespace event_camera_simulator { -/* The Simulator forwards the simulated images / depth maps - * from the data provider to multiple, specialized camera simulators, such as: - * (i) event camera simulators that simulate events based on sequences of images - * (ii) camera simulators that simulate real cameras - * (including motion blur, camera response function, noise, etc.) - * - * The Simulator then forwards the simulated data to one or more publishers. -*/ -class Simulator -{ -public: - Simulator(size_t num_cameras, + /* The Simulator forwards the simulated images / depth maps + * from the data provider to multiple, specialized camera simulators, such + * as: (i) event camera simulators that simulate events based on sequences + * of images (ii) camera simulators that simulate real cameras (including + * motion blur, camera response function, noise, etc.) + * + * The Simulator then forwards the simulated data to one or more publishers. + */ + class Simulator { + public: + Simulator( + size_t num_cameras, const EventSimulator::Config& event_sim_config, - double exposure_time_ms) - : num_cameras_(num_cameras), - exposure_time_(ze::millisecToNanosec(exposure_time_ms)) - { - for(size_t i=0; i event_simulators_; + private: + size_t num_cameras_; + std::vector event_simulators_; - std::vector camera_simulators_; - Duration exposure_time_; + std::vector camera_simulators_; + Duration exposure_time_; - std::vector publishers_; + std::vector publishers_; - ImagePtrVector corrupted_camera_images_; -}; + ImagePtrVector corrupted_camera_images_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim/src/camera_simulator.cpp b/event_camera_simulator/esim/src/camera_simulator.cpp index e1b08c7..4a276e3 100644 --- a/event_camera_simulator/esim/src/camera_simulator.cpp +++ b/event_camera_simulator/esim/src/camera_simulator.cpp @@ -88,7 +88,6 @@ namespace event_camera_simulator { << buffer_->getExposureTime() << std::endl; exposures_file_.close(); - *camera_image /= denom; cv::Mat disp; camera_image->convertTo(disp, CV_8U, 255); diff --git a/event_camera_simulator/esim/src/event_simulator.cpp b/event_camera_simulator/esim/src/event_simulator.cpp index a52911f..dd6c66a 100644 --- a/event_camera_simulator/esim/src/event_simulator.cpp +++ b/event_camera_simulator/esim/src/event_simulator.cpp @@ -1,119 +1,123 @@ #include -#include #include #include +#include #include namespace event_camera_simulator { -void EventSimulator::init(const Image &img, Time time) -{ - VLOG(1) << "Initialized event camera simulator with sensor size: " << img.size(); - VLOG(1) << "and contrast thresholds: C+ = " << config_.Cp << " , C- = " << config_.Cm; - is_initialized_ = true; - last_img_ = img.clone(); - ref_values_ = img.clone(); - last_event_timestamp_ = TimestampImage::zeros(img.size()); - current_time_ = time; - size_ = img.size(); -} + void EventSimulator::init(const Image& img, Time time) { + VLOG(1) << "Initialized event camera simulator with sensor size: " + << img.size(); + VLOG(1) << "and contrast thresholds: C+ = " << config_.Cp + << " , C- = " << config_.Cm; + is_initialized_ = true; + last_img_ = img.clone(); + ref_values_ = img.clone(); + last_event_timestamp_ = TimestampImage::zeros(img.size()); + current_time_ = time; + size_ = img.size(); + } -Events EventSimulator::imageCallback(const Image& img, Time time) -{ - CHECK_GE(time, 0); - Image preprocessed_img = img.clone(); - if(config_.use_log_image) - { - LOG_FIRST_N(INFO, 1) << "Converting the image to log image with eps = " << config_.log_eps << "."; - cv::log(config_.log_eps + img, preprocessed_img); - } - - if(!is_initialized_) - { - init(preprocessed_img, time); - return {}; - } - - // For each pixel, check if new events need to be generated since the last image sample - static constexpr ImageFloatType tolerance = 1e-6; - Events events; - Duration delta_t_ns = time - current_time_; - - CHECK_GT(delta_t_ns, 0u); - CHECK_EQ(img.size(), size_); - - for (int y = 0; y < size_.height; ++y) - { - for (int x = 0; x < size_.width; ++x) - { - ImageFloatType itdt = preprocessed_img(y, x); - ImageFloatType it = last_img_(y, x); - ImageFloatType prev_cross = ref_values_(y, x); - - if (std::fabs (it - itdt) > tolerance) - { - ImageFloatType pol = (itdt >= it) ? +1.0 : -1.0; - ImageFloatType C = (pol > 0) ? config_.Cp : config_.Cm; - ImageFloatType sigma_C = (pol > 0) ? config_.sigma_Cp : config_.sigma_Cm; - if(sigma_C > 0) - { - C += ze::sampleNormalDistribution(false, 0, sigma_C); - constexpr ImageFloatType minimum_contrast_threshold = 0.01; - C = std::max(minimum_contrast_threshold, C); + Events EventSimulator::imageCallback(const Image& img, Time time) { + CHECK_GE(time, 0); + Image preprocessed_img = img.clone(); + if (config_.use_log_image) { + LOG_FIRST_N(INFO, 1) + << "Converting the image to log image with eps = " + << config_.log_eps << "."; + cv::log(config_.log_eps + img, preprocessed_img); } - ImageFloatType curr_cross = prev_cross; - bool all_crossings = false; - do - { - curr_cross += pol * C; + if (!is_initialized_) { + init(preprocessed_img, time); + return {}; + } - if ((pol > 0 && curr_cross > it && curr_cross <= itdt) - || (pol < 0 && curr_cross < it && curr_cross >= itdt)) - { - Duration edt = (curr_cross - it) * delta_t_ns / (itdt - it); - Time t = current_time_ + edt; + // For each pixel, check if new events need to be generated since the + // last image sample + static constexpr ImageFloatType tolerance = 1e-6; + Events events; + Duration delta_t_ns = time - current_time_; - // check that pixel (x,y) is not currently in a "refractory" state - // i.e. |t-that last_timestamp(x,y)| >= refractory_period - const Time last_stamp_at_xy = ze::secToNanosec(last_event_timestamp_(y,x)); - CHECK_GE(t, last_stamp_at_xy); - const Duration dt = t - last_stamp_at_xy; - if(last_event_timestamp_(y,x) == 0 || dt >= config_.refractory_period_ns) - { - events.push_back(Event(x, y, t, pol > 0)); - last_event_timestamp_(y,x) = ze::nanosecToSecTrunc(t); - } - else - { - VLOG(3) << "Dropping event because time since last event (" - << dt << " ns) < refractory period (" - << config_.refractory_period_ns << " ns)."; - } - ref_values_(y,x) = curr_cross; - } - else - { - all_crossings = true; - } - } while (!all_crossings); - } // end tolerance - } // end for each pixel - } + CHECK_GT(delta_t_ns, 0u); + CHECK_EQ(img.size(), size_); - // update simvars for next loop - current_time_ = time; - last_img_ = preprocessed_img.clone(); // it is now the latest image + for (int y = 0; y < size_.height; ++y) { + for (int x = 0; x < size_.width; ++x) { + ImageFloatType itdt = preprocessed_img(y, x); + ImageFloatType it = last_img_(y, x); + ImageFloatType prev_cross = ref_values_(y, x); - // Sort the events by increasing timestamps, since this is what - // most event processing algorithms expect - sort(events.begin(), events.end(), - [](const Event& a, const Event& b) -> bool - { - return a.t < b.t; - }); + if (std::fabs(it - itdt) > tolerance) { + ImageFloatType pol = (itdt >= it) ? +1.0 : -1.0; + ImageFloatType C = (pol > 0) ? config_.Cp : config_.Cm; + ImageFloatType sigma_C = + (pol > 0) ? config_.sigma_Cp : config_.sigma_Cm; + if (sigma_C > 0) { + C += ze::sampleNormalDistribution( + false, + 0, + sigma_C + ); + constexpr ImageFloatType minimum_contrast_threshold = + 0.01; + C = std::max(minimum_contrast_threshold, C); + } + ImageFloatType curr_cross = prev_cross; + bool all_crossings = false; - return events; -} + do { + curr_cross += pol * C; + + if ((pol > 0 && curr_cross > it && curr_cross <= itdt) + || (pol < 0 && curr_cross < it && curr_cross >= itdt + )) { + Duration edt = + (curr_cross - it) * delta_t_ns / (itdt - it); + Time t = current_time_ + edt; + + // check that pixel (x,y) is not currently in a + // "refractory" state i.e. |t-that + // last_timestamp(x,y)| >= refractory_period + const Time last_stamp_at_xy = + ze::secToNanosec(last_event_timestamp_(y, x)); + CHECK_GE(t, last_stamp_at_xy); + const Duration dt = t - last_stamp_at_xy; + if (last_event_timestamp_(y, x) == 0 + || dt >= config_.refractory_period_ns) { + events.push_back(Event(x, y, t, pol > 0)); + last_event_timestamp_(y, x) = + ze::nanosecToSecTrunc(t); + } else { + VLOG(3) + << "Dropping event because time since last " + "event (" + << dt << " ns) < refractory period (" + << config_.refractory_period_ns << " ns)."; + } + ref_values_(y, x) = curr_cross; + } else { + all_crossings = true; + } + } while (!all_crossings); + } // end tolerance + } // end for each pixel + } + + // update simvars for next loop + current_time_ = time; + last_img_ = preprocessed_img.clone(); // it is now the latest image + + // Sort the events by increasing timestamps, since this is what + // most event processing algorithms expect + sort( + events.begin(), + events.end(), + [](const Event& a, const Event& b) -> bool { return a.t < b.t; } + ); + + return events; + } } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim/src/simulator.cpp b/event_camera_simulator/esim/src/simulator.cpp index 3ec4bbb..7bba668 100644 --- a/event_camera_simulator/esim/src/simulator.cpp +++ b/event_camera_simulator/esim/src/simulator.cpp @@ -1,143 +1,160 @@ +#include #include #include -#include namespace event_camera_simulator { -DECLARE_TIMER(TimerEventSimulator, timers_event_simulator_, - simulate_events, - visualization - ); + DECLARE_TIMER( + TimerEventSimulator, + timers_event_simulator_, + simulate_events, + visualization + ); -Simulator::~Simulator() -{ - timers_event_simulator_.saveToFile("/tmp", "event_simulator.csv"); -} + Simulator::~Simulator() { + timers_event_simulator_.saveToFile("/tmp", "event_simulator.csv"); + } -void Simulator::dataProviderCallback(const SimulatorData &sim_data) -{ - CHECK_EQ(event_simulators_.size(), num_cameras_); + void Simulator::dataProviderCallback(const SimulatorData& sim_data) { + CHECK_EQ(event_simulators_.size(), num_cameras_); - bool camera_simulator_success; + bool camera_simulator_success; - if(sim_data.images_updated) - { - EventsVector events(num_cameras_); - Time time = sim_data.timestamp; - // simulate the events and camera images for every sensor in the rig - { - auto t = timers_event_simulator_[TimerEventSimulator::simulate_events].timeScope(); - for(size_t i=0; i(sim_data.images[i]->size())); - corrupted_camera_images_[i]->setTo(0.); + if (corrupted_camera_images_.size() < num_cameras_) { + // allocate memory for the corrupted camera images and + // set them to 0 + corrupted_camera_images_.emplace_back( + std::make_shared(sim_data.images[i]->size()) + ); + corrupted_camera_images_[i]->setTo(0.); + } + + camera_simulator_success = + camera_simulators_[i].imageCallback( + *sim_data.images[i], + time, + corrupted_camera_images_[i] + ); + } + } + + // publish the simulation data + events + { + auto t = + timers_event_simulator_[TimerEventSimulator::visualization] + .timeScope(); + publishData( + sim_data, + events, + camera_simulator_success, + corrupted_camera_images_ + ); + } + } else { + { + // just forward the simulation data to the publisher + auto t = + timers_event_simulator_[TimerEventSimulator::visualization] + .timeScope(); + publishData( + sim_data, + {}, + camera_simulator_success, + corrupted_camera_images_ + ); + } + } + } + + void Simulator::publishData( + const SimulatorData& sim_data, + const EventsVector& events, + bool camera_simulator_success, + const ImagePtrVector& camera_images + ) { + if (publishers_.empty()) { + LOG_FIRST_N(WARNING, 1) << "No publisher available"; + return; } - camera_simulator_success = camera_simulators_[i].imageCallback(*sim_data.images[i], time, corrupted_camera_images_[i]); - } + Time time = sim_data.timestamp; + const Transformation& T_W_B = sim_data.groundtruth.T_W_B; + const TransformationVector& T_W_Cs = sim_data.groundtruth.T_W_Cs; + const ze::CameraRig::Ptr& camera_rig = sim_data.camera_rig; + + // Publish the new data (events, images, depth maps, poses, point + // clouds, etc.) + if (!events.empty()) + for (const Publisher::Ptr& publisher : publishers_) + publisher->eventsCallback(events); + if (sim_data.poses_updated) + for (const Publisher::Ptr& publisher : publishers_) + publisher->poseCallback(T_W_B, T_W_Cs, time); + if (sim_data.twists_updated) + for (const Publisher::Ptr& publisher : publishers_) + publisher->twistCallback( + sim_data.groundtruth.angular_velocities_, + sim_data.groundtruth.linear_velocities_, + time + ); + if (sim_data.imu_updated) + for (const Publisher::Ptr& publisher : publishers_) + publisher->imuCallback( + sim_data.specific_force_corrupted, + sim_data.angular_velocity_corrupted, + time + ); + + if (camera_rig) + for (const Publisher::Ptr& publisher : publishers_) + publisher->cameraInfoCallback(camera_rig, time); + if (sim_data.images_updated) { + for (const Publisher::Ptr& publisher : publishers_) { + publisher->imageCallback(sim_data.images, time); + + if (camera_simulator_success && time >= exposure_time_) { + // the images should be timestamped at mid-exposure + const Time mid_exposure_time = time - 0.5 * exposure_time_; + publisher->imageCorruptedCallback( + camera_images, + mid_exposure_time + ); + } + } + } + if (sim_data.depthmaps_updated) + for (const Publisher::Ptr& publisher : publishers_) + publisher->depthmapCallback(sim_data.depthmaps, time); + if (sim_data.optic_flows_updated) + for (const Publisher::Ptr& publisher : publishers_) + publisher->opticFlowCallback(sim_data.optic_flows, time); + if (sim_data.depthmaps_updated && !events.empty()) { + PointCloudVector pointclouds(num_cameras_); + for (size_t i = 0; i < num_cameras_; ++i) { + CHECK(sim_data.depthmaps[i]); + pointclouds[i] = eventsToPointCloud( + events[i], + *sim_data.depthmaps[i], + camera_rig->atShared(i) + ); + } + for (const Publisher::Ptr& publisher : publishers_) + publisher->pointcloudCallback(pointclouds, time); + } } - // publish the simulation data + events - { - auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope(); - publishData(sim_data, events, camera_simulator_success, corrupted_camera_images_); - } - } - else - { - { - // just forward the simulation data to the publisher - auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope(); - publishData(sim_data, {}, camera_simulator_success, corrupted_camera_images_); - } - } -} - -void Simulator::publishData(const SimulatorData& sim_data, - const EventsVector& events, - bool camera_simulator_success, - const ImagePtrVector& camera_images) -{ - if(publishers_.empty()) - { - LOG_FIRST_N(WARNING, 1) << "No publisher available"; - return; - } - - Time time = sim_data.timestamp; - const Transformation& T_W_B = sim_data.groundtruth.T_W_B; - const TransformationVector& T_W_Cs = sim_data.groundtruth.T_W_Cs; - const ze::CameraRig::Ptr& camera_rig = sim_data.camera_rig; - - // Publish the new data (events, images, depth maps, poses, point clouds, etc.) - if(!events.empty()) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->eventsCallback(events); - } - if(sim_data.poses_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->poseCallback(T_W_B, T_W_Cs, time); - } - if(sim_data.twists_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->twistCallback(sim_data.groundtruth.angular_velocities_, - sim_data.groundtruth.linear_velocities_, - time); - } - if(sim_data.imu_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->imuCallback(sim_data.specific_force_corrupted, sim_data.angular_velocity_corrupted, time); - } - - if(camera_rig) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->cameraInfoCallback(camera_rig, time); - } - if(sim_data.images_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - { - publisher->imageCallback(sim_data.images, time); - - if(camera_simulator_success && time >= exposure_time_) - { - // the images should be timestamped at mid-exposure - const Time mid_exposure_time = time - 0.5 * exposure_time_; - publisher->imageCorruptedCallback(camera_images, mid_exposure_time); - } - } - } - if(sim_data.depthmaps_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->depthmapCallback(sim_data.depthmaps, time); - } - if(sim_data.optic_flows_updated) - { - for(const Publisher::Ptr& publisher : publishers_) - publisher->opticFlowCallback(sim_data.optic_flows, time); - } - if(sim_data.depthmaps_updated && !events.empty()) - { - PointCloudVector pointclouds(num_cameras_); - for(size_t i=0; iatShared(i)); - } - for(const Publisher::Ptr& publisher : publishers_) - publisher->pointcloudCallback(pointclouds, time); - } -} - } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim/test/test_event_simulator.cpp b/event_camera_simulator/esim/test/test_event_simulator.cpp index 0137140..91633f5 100644 --- a/event_camera_simulator/esim/test/test_event_simulator.cpp +++ b/event_camera_simulator/esim/test/test_event_simulator.cpp @@ -1,237 +1,225 @@ #include -#include #include +#include #include #include #include -#include - +#include #include #include -#include #define USE_OPENCV -namespace event_camera_simulator -{ +namespace event_camera_simulator { -class CSVImageLoader -{ -public: - CSVImageLoader(const std::string& path_to_data_folder) - : path_to_data_folder_(path_to_data_folder) - { - images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv")); - CHECK(images_in_str_.is_open()); - } + class CSVImageLoader { + public: + CSVImageLoader(const std::string& path_to_data_folder) + : path_to_data_folder_(path_to_data_folder) { + images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv") + ); + CHECK(images_in_str_.is_open()); + } - bool next(int64_t& stamp, Image& img) - { - std::string line; - if(!getline(images_in_str_, line)) - { - LOG(INFO) << "Finished reading all the images in the folder"; - return false; - } + bool next(int64_t& stamp, Image& img) { + std::string line; + if (!getline(images_in_str_, line)) { + LOG(INFO) << "Finished reading all the images in the folder"; + return false; + } - if('%' != line.at(0) && '#' != line.at(0)) - { - std::vector items = ze::splitString(line, delimiter_); - stamp = std::stoll(items[0]); + if ('%' != line.at(0) && '#' != line.at(0)) { + std::vector items = + ze::splitString(line, delimiter_); + stamp = std::stoll(items[0]); - const std::string& path_to_image - = ze::joinPath(path_to_data_folder_, "frame", "cam_0", items[1]); + const std::string& path_to_image = ze::joinPath( + path_to_data_folder_, + "frame", + "cam_0", + items[1] + ); - img = cv::imread(path_to_image, 0); - CHECK(img.data) << "Error loading image: " << path_to_image; + img = cv::imread(path_to_image, 0); + CHECK(img.data) << "Error loading image: " << path_to_image; - return true; - } - else - { - return next(stamp, img); - } - } + return true; + } else { + return next(stamp, img); + } + } -private: - std::ifstream images_in_str_; - const char delimiter_{','}; - std::string path_to_data_folder_; -}; + private: + std::ifstream images_in_str_; + const char delimiter_{','}; + std::string path_to_data_folder_; + }; } // namespace event_camera_simulator -std::string getTestDataDir(const std::string& dataset_name) -{ - using namespace ze; +std::string getTestDataDir(const std::string& dataset_name) { + using namespace ze; - const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH"); - CHECK(datapath_dir != nullptr) - << "Did you download the esim_test_data repository and set " - << "the ESIM_TEST_DATA_PATH environment variable?"; + const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH"); + CHECK(datapath_dir != nullptr) + << "Did you download the esim_test_data repository and set " + << "the ESIM_TEST_DATA_PATH environment variable?"; - std::string path(datapath_dir); - CHECK(isDir(path)) << "Folder does not exist: " << path; - path = path + "/data/" + dataset_name; - CHECK(isDir(path)) << "Dataset does not exist: " << path; - return path; + std::string path(datapath_dir); + CHECK(isDir(path)) << "Folder does not exist: " << path; + path = path + "/data/" + dataset_name; + CHECK(isDir(path)) << "Dataset does not exist: " << path; + return path; } -TEST(EventSimulator, testImageReconstruction) -{ - using namespace event_camera_simulator; +TEST(EventSimulator, testImageReconstruction) { + using namespace event_camera_simulator; - // Load image sequence from folder - const std::string path_to_data_folder = getTestDataDir("planar_carpet"); - CSVImageLoader reader(path_to_data_folder); + // Load image sequence from folder + const std::string path_to_data_folder = getTestDataDir("planar_carpet"); + CSVImageLoader reader(path_to_data_folder); - EventSimulator::Config event_sim_config; - event_sim_config.Cp = 0.05; - event_sim_config.Cm = 0.03; - event_sim_config.sigma_Cp = 0; - event_sim_config.sigma_Cm = 0; - event_sim_config.use_log_image = true; - event_sim_config.log_eps = 0.001; - EventSimulator simulator(event_sim_config); + EventSimulator::Config event_sim_config; + event_sim_config.Cp = 0.05; + event_sim_config.Cm = 0.03; + event_sim_config.sigma_Cp = 0; + event_sim_config.sigma_Cm = 0; + event_sim_config.use_log_image = true; + event_sim_config.log_eps = 0.001; + EventSimulator simulator(event_sim_config); - LOG(INFO) << "Testing event camera simulator with C+ = " << event_sim_config.Cp - << ", C- = " << event_sim_config.Cm; + LOG(INFO) << "Testing event camera simulator with C+ = " + << event_sim_config.Cp << ", C- = " << event_sim_config.Cm; - const ImageFloatType max_reconstruction_error - = std::max(event_sim_config.Cp, event_sim_config.Cm); + const ImageFloatType max_reconstruction_error = + std::max(event_sim_config.Cp, event_sim_config.Cm); - bool is_first_image = true; - Image I, L, L_reconstructed; - int64_t stamp; - while(reader.next(stamp, I)) - { - I.convertTo(I, cv::DataType::type, 1./255.); - cv::log(event_sim_config.log_eps + I, L); + bool is_first_image = true; + Image I, L, L_reconstructed; + int64_t stamp; + while (reader.next(stamp, I)) { + I.convertTo(I, cv::DataType::type, 1. / 255.); + cv::log(event_sim_config.log_eps + I, L); - if(is_first_image) - { - // Initialize reconstructed image from the ground truth image - L_reconstructed = L.clone(); - is_first_image = false; - } + if (is_first_image) { + // Initialize reconstructed image from the ground truth image + L_reconstructed = L.clone(); + is_first_image = false; + } - Events events = simulator.imageCallback(I, stamp); + Events events = simulator.imageCallback(I, stamp); - // Reconstruct next image from previous one using the events in between - for(const Event& e : events) - { - ImageFloatType pol = e.pol ? 1. : -1.; - const ImageFloatType C = e.pol ? event_sim_config.Cp : event_sim_config.Cm; - L_reconstructed(e.y,e.x) += pol * C; - } + // Reconstruct next image from previous one using the events in between + for (const Event& e : events) { + ImageFloatType pol = e.pol ? 1. : -1.; + const ImageFloatType C = + e.pol ? event_sim_config.Cp : event_sim_config.Cm; + L_reconstructed(e.y, e.x) += pol * C; + } - // Check that the reconstruction error is bounded by the contrast thresholds - for(int y=0; y::type, 1. / 255.); + cv::log(event_sim_config.log_eps + I, L); - std::vector times, mean_errors, stddev_errors; - bool is_first_image = true; - Image I, L, L_reconstructed; - int64_t stamp; - while(reader.next(stamp, I)) - { - LOG_EVERY_N(INFO, 50) << "t = " << ze::nanosecToSecTrunc(stamp) << " s"; - I.convertTo(I, cv::DataType::type, 1./255.); - cv::log(event_sim_config.log_eps + I, L); + if (is_first_image) { + // Initialize reconstructed image from the ground truth image + L_reconstructed = L.clone(); + is_first_image = false; + } - if(is_first_image) - { - // Initialize reconstructed image from the ground truth image - L_reconstructed = L.clone(); - is_first_image = false; - } + Events events = simulator.imageCallback(I, stamp); - Events events = simulator.imageCallback(I, stamp); + // Reconstruct next image from previous one using the events in between + for (const Event& e : events) { + ImageFloatType pol = e.pol ? 1. : -1.; + ImageFloatType C = + e.pol ? event_sim_config.Cp : event_sim_config.Cm; + C += contrast_bias; + L_reconstructed(e.y, e.x) += pol * C; + } - // Reconstruct next image from previous one using the events in between - for(const Event& e : events) - { - ImageFloatType pol = e.pol ? 1. : -1.; - ImageFloatType C = e.pol ? event_sim_config.Cp : event_sim_config.Cm; - C += contrast_bias; - L_reconstructed(e.y,e.x) += pol * C; - } + // Compute the mean and average reconstruction error over the whole + // image + Image error; + cv::absdiff(L, L_reconstructed, error); + cv::Scalar mean_error, stddev_error; + cv::meanStdDev(error, mean_error, stddev_error); + VLOG(1) << "Mean error: " << mean_error << ", Stddev: " << stddev_error; - // Compute the mean and average reconstruction error over the whole image - Image error; - cv::absdiff(L, L_reconstructed, error); - cv::Scalar mean_error, stddev_error; - cv::meanStdDev(error, mean_error, stddev_error); - VLOG(1) << "Mean error: " << mean_error - << ", Stddev: " << stddev_error; - - times.push_back(ze::nanosecToSecTrunc(stamp)); - mean_errors.push_back(mean_error[0]); - stddev_errors.push_back(stddev_error[0]); + times.push_back(ze::nanosecToSecTrunc(stamp)); + mean_errors.push_back(mean_error[0]); + stddev_errors.push_back(stddev_error[0]); #ifdef USE_OPENCV - const ImageFloatType vmin = std::log(event_sim_config.log_eps); - const ImageFloatType vmax = std::log(1.0 + event_sim_config.log_eps); - cv::Mat disp = 255.0 * (L_reconstructed - vmin) / (vmax - vmin); - disp.convertTo(disp, CV_8U); - cv::imshow("Reconstructed", disp); - cv::waitKey(1); + const ImageFloatType vmin = std::log(event_sim_config.log_eps); + const ImageFloatType vmax = std::log(1.0 + event_sim_config.log_eps); + cv::Mat disp = 255.0 * (L_reconstructed - vmin) / (vmax - vmin); + disp.convertTo(disp, CV_8U); + cv::imshow("Reconstructed", disp); + cv::waitKey(1); #endif - } + } - // Plot the mean and stddev reconstruction error over time - ze::plt::plot(times, mean_errors, "r"); - ze::plt::plot(times, stddev_errors, "b--"); - std::stringstream title; - title << "C = " << event_sim_config.Cp - << ", sigma = " << event_sim_config.sigma_Cp - << ", bias = " << contrast_bias; - ze::plt::title(title.str()); - ze::plt::xlabel("Time (s)"); - ze::plt::ylabel("Mean / Stddev reconstruction error"); - ze::plt::grid(true); - ze::plt::save("/tmp/evolution_reconstruction_error.pdf"); - ze::plt::show(); + // Plot the mean and stddev reconstruction error over time + ze::plt::plot(times, mean_errors, "r"); + ze::plt::plot(times, stddev_errors, "b--"); + std::stringstream title; + title << "C = " << event_sim_config.Cp + << ", sigma = " << event_sim_config.sigma_Cp + << ", bias = " << contrast_bias; + ze::plt::title(title.str()); + ze::plt::xlabel("Time (s)"); + ze::plt::ylabel("Mean / Stddev reconstruction error"); + ze::plt::grid(true); + ze::plt::save("/tmp/evolution_reconstruction_error.pdf"); + ze::plt::show(); } ZE_UNITTEST_ENTRYPOINT diff --git a/event_camera_simulator/esim_common/include/esim/common/types.hpp b/event_camera_simulator/esim_common/include/esim/common/types.hpp index e174326..6d41352 100644 --- a/event_camera_simulator/esim_common/include/esim/common/types.hpp +++ b/event_camera_simulator/esim_common/include/esim/common/types.hpp @@ -1,160 +1,155 @@ #pragma once -#include +#include #include #include -#include +#include // FloatType defines the floating point accuracy (single or double precision) -// for the geometric operations (computing rotation matrices, point projection, etc.). -// This should typically be double precision (highest accuracy). +// for the geometric operations (computing rotation matrices, point projection, +// etc.). This should typically be double precision (highest accuracy). #define FloatType ze::real_t -// ImageFloatType defines the floating point accuracy (single or double precision) -// of the intensity images (and depth images). -// Single precision should be enough there in most cases. +// ImageFloatType defines the floating point accuracy (single or double +// precision) of the intensity images (and depth images). Single precision +// should be enough there in most cases. #define ImageFloatType float namespace event_camera_simulator { -using Translation = ze::Position; -using Vector3 = ze::Vector3; -using Vector4 = ze::Vector4; -using Vector3i = Eigen::Vector3i; + using Translation = ze::Position; + using Vector3 = ze::Vector3; + using Vector4 = ze::Vector4; + using Vector3i = Eigen::Vector3i; -using Transformation = ze::Transformation; -using TransformationVector = ze::TransformationVector; -using TransformationPtr = std::shared_ptr; + using Transformation = ze::Transformation; + using TransformationVector = ze::TransformationVector; + using TransformationPtr = std::shared_ptr; -using Normal = ze::Vector3; -using CalibrationMatrix = ze::Matrix3; -using RotationMatrix = ze::Matrix3; -using HomographyMatrix = ze::Matrix3; + using Normal = ze::Vector3; + using CalibrationMatrix = ze::Matrix3; + using RotationMatrix = ze::Matrix3; + using HomographyMatrix = ze::Matrix3; -using AngularVelocity = ze::Vector3; -using LinearVelocity = ze::Vector3; -using AngularVelocityVector = std::vector; -using LinearVelocityVector = std::vector; + using AngularVelocity = ze::Vector3; + using LinearVelocity = ze::Vector3; + using AngularVelocityVector = std::vector; + using LinearVelocityVector = std::vector; -using uint16_t = ze::uint16_t; + using uint16_t = ze::uint16_t; -using Time = ze::int64_t; -using Duration = ze::uint64_t; -using Image = cv::Mat_; -using ImagePtr = std::shared_ptr; -using Depthmap = cv::Mat_; -using OpticFlow = cv::Mat_< cv::Vec >; -using OpticFlowPtr = std::shared_ptr; -using DepthmapPtr = std::shared_ptr; + using Time = ze::int64_t; + using Duration = ze::uint64_t; + using Image = cv::Mat_; + using ImagePtr = std::shared_ptr; + using Depthmap = cv::Mat_; + using OpticFlow = cv::Mat_>; + using OpticFlowPtr = std::shared_ptr; + using DepthmapPtr = std::shared_ptr; -using ImagePtrVector = std::vector; -using DepthmapPtrVector = std::vector; -using OpticFlowPtrVector = std::vector; + using ImagePtrVector = std::vector; + using DepthmapPtrVector = std::vector; + using OpticFlowPtrVector = std::vector; -using Camera = ze::Camera; + using Camera = ze::Camera; -struct Event -{ - Event(uint16_t x, uint16_t y, Time t, bool pol) - : x(x), - y(y), - t(t), - pol(pol) - { + struct Event { + Event(uint16_t x, uint16_t y, Time t, bool pol) + : x(x), + y(y), + t(t), + pol(pol) {} - } + uint16_t x; + uint16_t y; + Time t; + bool pol; + }; - uint16_t x; - uint16_t y; - Time t; - bool pol; -}; + using Events = std::vector; + using EventsVector = std::vector; + using EventsPtr = std::shared_ptr; -using Events = std::vector; -using EventsVector = std::vector; -using EventsPtr = std::shared_ptr; + struct PointXYZRGB { + PointXYZRGB( + FloatType x, FloatType y, FloatType z, int red, int green, int blue + ) + : xyz(x, y, z), + rgb(red, green, blue) {} -struct PointXYZRGB -{ - PointXYZRGB(FloatType x, FloatType y, FloatType z, - int red, int green, int blue) - : xyz(x, y, z), - rgb(red, green, blue) {} + PointXYZRGB(const Vector3& xyz): xyz(xyz) {} - PointXYZRGB(const Vector3& xyz) - : xyz(xyz) {} + PointXYZRGB(const Vector3& xyz, const Vector3i& rgb) + : xyz(xyz), + rgb(rgb) {} - PointXYZRGB(const Vector3& xyz, const Vector3i& rgb) - : xyz(xyz), - rgb(rgb) {} + Vector3 xyz; + Vector3i rgb; + }; - Vector3 xyz; - Vector3i rgb; -}; + using PointCloud = std::vector; + using PointCloudVector = std::vector; -using PointCloud = std::vector; -using PointCloudVector = std::vector; + struct SimulatorData { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW -struct SimulatorData -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + //! Nanosecond timestamp. + Time timestamp; - //! Nanosecond timestamp. - Time timestamp; + //! Camera images. + ImagePtrVector images; - //! Camera images. - ImagePtrVector images; + //! Depth maps. + DepthmapPtrVector depthmaps; - //! Depth maps. - DepthmapPtrVector depthmaps; + //! Optic flow maps. + OpticFlowPtrVector optic_flows; - //! Optic flow maps. - OpticFlowPtrVector optic_flows; + //! Camera + ze::CameraRig::Ptr camera_rig; - //! Camera - ze::CameraRig::Ptr camera_rig; + //! An accelerometer measures the specific force (incl. gravity), + //! corrupted by noise and bias. + Vector3 specific_force_corrupted; - //! An accelerometer measures the specific force (incl. gravity), - //! corrupted by noise and bias. - Vector3 specific_force_corrupted; + //! The angular velocity (in the body frame) corrupted by noise and + //! bias. + Vector3 angular_velocity_corrupted; - //! The angular velocity (in the body frame) corrupted by noise and bias. - Vector3 angular_velocity_corrupted; + //! Groundtruth states. + struct Groundtruth { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - //! Groundtruth states. - struct Groundtruth - { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + //! Pose of the body (i.e. the IMU) expressed in the world frame. + Transformation T_W_B; - //! Pose of the body (i.e. the IMU) expressed in the world frame. - Transformation T_W_B; + //! Accelerometer and gyro bias + Vector3 acc_bias; + Vector3 gyr_bias; - //! Accelerometer and gyro bias - Vector3 acc_bias; - Vector3 gyr_bias; + //! Poses of the cameras in the rig expressed in the world frame. + TransformationVector T_W_Cs; - //! Poses of the cameras in the rig expressed in the world frame. - TransformationVector T_W_Cs; + //! Linear and angular velocities (i.e. twists) of the cameras in + //! the rig, expressed in each camera's local coordinate frame. + LinearVelocityVector linear_velocities_; + AngularVelocityVector angular_velocities_; - //! Linear and angular velocities (i.e. twists) of the cameras in the rig, - //! expressed in each camera's local coordinate frame. - LinearVelocityVector linear_velocities_; - AngularVelocityVector angular_velocities_; + // dynamic objects + std::vector T_W_OBJ_; + std::vector linear_velocity_obj_; + std::vector angular_velocity_obj_; + }; - // dynamic objects - std::vector T_W_OBJ_; - std::vector linear_velocity_obj_; - std::vector angular_velocity_obj_; - }; - Groundtruth groundtruth; + Groundtruth groundtruth; - // Flags to indicate whether a value has been updated or not - bool images_updated; - bool depthmaps_updated; - bool optic_flows_updated; - bool twists_updated; - bool poses_updated; - bool imu_updated; -}; + // Flags to indicate whether a value has been updated or not + bool images_updated; + bool depthmaps_updated; + bool optic_flows_updated; + bool twists_updated; + bool poses_updated; + bool imu_updated; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_common/include/esim/common/utils.hpp b/event_camera_simulator/esim_common/include/esim/common/utils.hpp index d89e7be..fceae5d 100644 --- a/event_camera_simulator/esim_common/include/esim/common/utils.hpp +++ b/event_camera_simulator/esim_common/include/esim/common/utils.hpp @@ -3,57 +3,70 @@ #include namespace ze { - class Camera; + class Camera; } namespace event_camera_simulator { -inline double degToRad(double deg) -{ - return deg * CV_PI / 180.0; -} + inline double degToRad(double deg) { + return deg * CV_PI / 180.0; + } -inline double hfovToFocalLength(double hfov_deg, int W) -{ - return 0.5 * static_cast(W) / std::tan(0.5 * degToRad(hfov_deg)); -} + inline double hfovToFocalLength(double hfov_deg, int W) { + return 0.5 * static_cast(W) + / std::tan(0.5 * degToRad(hfov_deg)); + } -CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera); + CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera); -PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, const ze::Camera::Ptr& camera); + PointCloud eventsToPointCloud( + const Events& events, + const Depthmap& depthmap, + const ze::Camera::Ptr& camera + ); -FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow); + FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow); -FloatType maxPredictedAbsBrightnessChange(const ImagePtr& I, const OpticFlowPtr& flow); + FloatType maxPredictedAbsBrightnessChange( + const ImagePtr& I, const OpticFlowPtr& flow + ); -void gaussianBlur(ImagePtr& I, FloatType sigma); + void gaussianBlur(ImagePtr& I, FloatType sigma); -// Helper class to compute optic flow from a twist vector and depth map -// Precomputes a lookup table for pixel -> bearing vector correspondences -// to accelerate the computation -class OpticFlowHelper -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // Helper class to compute optic flow from a twist vector and depth map + // Precomputes a lookup table for pixel -> bearing vector correspondences + // to accelerate the computation + class OpticFlowHelper { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ZE_POINTER_TYPEDEFS(OpticFlowHelper); + ZE_POINTER_TYPEDEFS(OpticFlowHelper); - OpticFlowHelper(const ze::Camera::Ptr& camera); + OpticFlowHelper(const ze::Camera::Ptr& camera); - void computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, - const DepthmapPtr& depthmap, OpticFlowPtr& flow); + void computeFromDepthAndTwist( + const ze::Vector3& w_WC, + const ze::Vector3& v_WC, + const DepthmapPtr& depthmap, + OpticFlowPtr& flow + ); - void computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap, - const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ, - OpticFlowPtr& flow); + void computeFromDepthCamTwistAndObjDepthAndTwist( + const ze::Vector3& w_WC, + const ze::Vector3& v_WC, + const DepthmapPtr& depthmap, + const ze::Vector3& r_COBJ, + const ze::Vector3& w_WOBJ, + const ze::Vector3& v_WOBJ, + OpticFlowPtr& flow + ); -private: + private: + void precomputePixelToBearingLookupTable(); - void precomputePixelToBearingLookupTable(); + ze::Camera::Ptr camera_; - ze::Camera::Ptr camera_; - - // Precomputed lookup table from keypoint -> bearing vector - ze::Bearings bearings_C_; -}; + // Precomputed lookup table from keypoint -> bearing vector + ze::Bearings bearings_C_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_common/src/utils.cpp b/event_camera_simulator/esim_common/src/utils.cpp index 7b28edc..93b51e4 100644 --- a/event_camera_simulator/esim_common/src/utils.cpp +++ b/event_camera_simulator/esim_common/src/utils.cpp @@ -1,190 +1,212 @@ #include -#include #include +#include namespace event_camera_simulator { -OpticFlowHelper::OpticFlowHelper(const ze::Camera::Ptr& camera) - : camera_(camera) -{ - CHECK(camera_); - precomputePixelToBearingLookupTable(); -} - -void OpticFlowHelper::precomputePixelToBearingLookupTable() -{ - // points_C is a matrix containing the coordinates of each pixel coordinate in the image plane - ze::Keypoints points_C(2, camera_->width() * camera_->height()); - for(int y=0; yheight(); ++y) - { - for(int x=0; xwidth(); ++x) - { - points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y); + OpticFlowHelper::OpticFlowHelper(const ze::Camera::Ptr& camera) + : camera_(camera) { + CHECK(camera_); + precomputePixelToBearingLookupTable(); } - } - bearings_C_ = camera_->backProjectVectorized(points_C); - bearings_C_.array().rowwise() /= bearings_C_.row(2).array(); -} -void OpticFlowHelper::computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, - const DepthmapPtr& depthmap, OpticFlowPtr& flow) -{ - CHECK(depthmap); - CHECK_EQ(depthmap->rows, camera_->height()); - CHECK_EQ(depthmap->cols, camera_->width()); - CHECK(depthmap->isContinuous()); - - const ze::Vector3 w_CW = -w_WC; // rotation speed of the world wrt the camera - const ze::Vector3 v_CW = -v_WC; // speed of the world wrt the camera - const ze::Matrix33 R_CW = ze::skewSymmetric(w_CW); - - Eigen::Map> depths(depthmap->ptr(), 1, depthmap->rows * depthmap->cols); - ze::Positions Xs = bearings_C_; - Xs.array().rowwise() *= depths.cast().array(); - - ze::Matrix6X dproject_dX = - camera_->dProject_dLandmarkVectorized(Xs); - - for(int y=0; yheight(); ++y) - { - for(int x=0; xwidth(); ++x) - { - const Vector3 X = Xs.col(x + camera_->width() * y); - ze::Matrix31 dXdt = R_CW * X + v_CW; - ze::Vector2 flow_vec - = Eigen::Map(dproject_dX.col(x + camera_->width() * y).data()) * dXdt; - - (*flow)(y,x) = cv::Vec(flow_vec(0), flow_vec(1)); + void OpticFlowHelper::precomputePixelToBearingLookupTable() { + // points_C is a matrix containing the coordinates of each pixel + // coordinate in the image plane + ze::Keypoints points_C(2, camera_->width() * camera_->height()); + for (int y = 0; y < camera_->height(); ++y) + for (int x = 0; x < camera_->width(); ++x) + points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y); + bearings_C_ = camera_->backProjectVectorized(points_C); + bearings_C_.array().rowwise() /= bearings_C_.row(2).array(); } - } -} -void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap, - const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ, OpticFlowPtr& flow) -{ - CHECK(depthmap); - CHECK_EQ(depthmap->rows, camera_->height()); - CHECK_EQ(depthmap->cols, camera_->width()); - CHECK(depthmap->isContinuous()); + void OpticFlowHelper::computeFromDepthAndTwist( + const ze::Vector3& w_WC, + const ze::Vector3& v_WC, + const DepthmapPtr& depthmap, + OpticFlowPtr& flow + ) { + CHECK(depthmap); + CHECK_EQ(depthmap->rows, camera_->height()); + CHECK_EQ(depthmap->cols, camera_->width()); + CHECK(depthmap->isContinuous()); - const ze::Matrix33 w_WC_tilde = ze::skewSymmetric(w_WC); - const ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(w_WOBJ); + const ze::Vector3 w_CW = + -w_WC; // rotation speed of the world wrt the camera + const ze::Vector3 v_CW = -v_WC; // speed of the world wrt the camera + const ze::Matrix33 R_CW = ze::skewSymmetric(w_CW); - Eigen::Map> depths(depthmap->ptr(), 1, depthmap->rows * depthmap->cols); - ze::Positions Xs = bearings_C_; - Xs.array().rowwise() *= depths.cast().array(); + Eigen::Map< + const Eigen:: + Matrix> + depths( + depthmap->ptr(), + 1, + depthmap->rows * depthmap->cols + ); + ze::Positions Xs = bearings_C_; + Xs.array().rowwise() *= depths.cast().array(); - ze::Matrix6X dproject_dX = - camera_->dProject_dLandmarkVectorized(Xs); + ze::Matrix6X dproject_dX = camera_->dProject_dLandmarkVectorized(Xs); - for(int y=0; yheight(); ++y) - { - for(int x=0; xwidth(); ++x) - { - const Vector3 r_CX = Xs.col(x + camera_->width() * y); - const Vector3 r_OBJX = r_CX - r_COBJ; + for (int y = 0; y < camera_->height(); ++y) { + for (int x = 0; x < camera_->width(); ++x) { + const Vector3 X = Xs.col(x + camera_->width() * y); + ze::Matrix31 dXdt = R_CW * X + v_CW; + ze::Vector2 flow_vec = + Eigen::Map( + dproject_dX.col(x + camera_->width() * y).data() + ) + * dXdt; - ze::Matrix31 dXdt = v_WOBJ - v_WC - w_WC_tilde*r_CX + w_WOBJ_tilde*r_OBJX; - ze::Vector2 flow_vec - = Eigen::Map(dproject_dX.col(x + camera_->width() * y).data()) * dXdt; - - (*flow)(y,x) = cv::Vec(flow_vec(0), flow_vec(1)); + (*flow)(y, x) = cv::Vec(flow_vec(0), flow_vec(1)); + } + } } - } -} -FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow) -{ - CHECK(flow); - FloatType max_squared_magnitude = 0; - for(int y=0; yrows; ++y) - { - for(int x=0; xcols; ++x) - { - const FloatType squared_magnitude = cv::norm((*flow)(y,x), cv::NORM_L2SQR); - if(squared_magnitude > max_squared_magnitude) - { - max_squared_magnitude = squared_magnitude; - } + void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist( + const ze::Vector3& w_WC, + const ze::Vector3& v_WC, + const DepthmapPtr& depthmap, + const ze::Vector3& r_COBJ, + const ze::Vector3& w_WOBJ, + const ze::Vector3& v_WOBJ, + OpticFlowPtr& flow + ) { + CHECK(depthmap); + CHECK_EQ(depthmap->rows, camera_->height()); + CHECK_EQ(depthmap->cols, camera_->width()); + CHECK(depthmap->isContinuous()); + + const ze::Matrix33 w_WC_tilde = ze::skewSymmetric(w_WC); + const ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(w_WOBJ); + + Eigen::Map< + const Eigen:: + Matrix> + depths( + depthmap->ptr(), + 1, + depthmap->rows * depthmap->cols + ); + ze::Positions Xs = bearings_C_; + Xs.array().rowwise() *= depths.cast().array(); + + ze::Matrix6X dproject_dX = camera_->dProject_dLandmarkVectorized(Xs); + + for (int y = 0; y < camera_->height(); ++y) { + for (int x = 0; x < camera_->width(); ++x) { + const Vector3 r_CX = Xs.col(x + camera_->width() * y); + const Vector3 r_OBJX = r_CX - r_COBJ; + + ze::Matrix31 dXdt = + v_WOBJ - v_WC - w_WC_tilde * r_CX + w_WOBJ_tilde * r_OBJX; + ze::Vector2 flow_vec = + Eigen::Map( + dproject_dX.col(x + camera_->width() * y).data() + ) + * dXdt; + + (*flow)(y, x) = cv::Vec(flow_vec(0), flow_vec(1)); + } + } } - } - return std::sqrt(max_squared_magnitude); -} -FloatType maxPredictedAbsBrightnessChange(const ImagePtr& I, const OpticFlowPtr& flow) -{ - const size_t height = I->rows; - const size_t width = I->cols; - - Image Ix, Iy; // horizontal/vertical gradients of I - // the factor 1/8 accounts for the scaling introduced by the Sobel filter mask - //cv::Sobel(*I, Ix, cv::DataType::type, 1, 0, 3, 1./8.); - //cv::Sobel(*I, Iy, cv::DataType::type, 0, 1, 3, 1./8.); - - // the factor 1/32 accounts for the scaling introduced by the Scharr filter mask - cv::Scharr(*I, Ix, cv::DataType::type, 1, 0, 1./32.); - cv::Scharr(*I, Iy, cv::DataType::type, 0, 1, 1./32.); - - Image Lx, Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I * dI/dx - static const ImageFloatType eps = 1e-3; // small additive term to avoid problems at I=0 - cv::divide(Ix, *I+eps, Lx); - cv::divide(Iy, *I+eps, Ly); - - Image dLdt(height, width); - for(int y=0; y - const ImageFloatType dLdt_at_xy = - Lx(y,x) * (*flow)(y,x)[0] + - Ly(y,x) * (*flow)(y,x)[1]; // "-" sign ignored since we are interested in the absolute value... - dLdt(y,x) = std::fabs(dLdt_at_xy); + FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow) { + CHECK(flow); + FloatType max_squared_magnitude = 0; + for (int y = 0; y < flow->rows; ++y) { + for (int x = 0; x < flow->cols; ++x) { + const FloatType squared_magnitude = + cv::norm((*flow)(y, x), cv::NORM_L2SQR); + if (squared_magnitude > max_squared_magnitude) + max_squared_magnitude = squared_magnitude; + } + } + return std::sqrt(max_squared_magnitude); } - } - double min_dLdt, max_dLdt; - int min_idx, max_idx; - cv::minMaxIdx(dLdt, &min_dLdt, &max_dLdt, - &min_idx, &max_idx); - return static_cast(max_dLdt); -} + FloatType maxPredictedAbsBrightnessChange( + const ImagePtr& I, const OpticFlowPtr& flow + ) { + const size_t height = I->rows; + const size_t width = I->cols; -void gaussianBlur(ImagePtr& I, FloatType sigma) -{ - cv::GaussianBlur(*I, *I, cv::Size(15,15), sigma, sigma); -} + Image Ix, Iy; // horizontal/vertical gradients of I + // the factor 1/8 accounts for the scaling introduced by the Sobel + // filter mask cv::Sobel(*I, Ix, cv::DataType::type, 1, + // 0, 3, 1./8.); cv::Sobel(*I, Iy, cv::DataType::type, + // 0, 1, 3, 1./8.); -CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera) -{ - CHECK(camera); - const ze::VectorX params = camera->projectionParameters(); - CalibrationMatrix K; - K << params(0), 0, params(2), - 0, params(1), params(3), - 0, 0, 1; - return K; -} + // the factor 1/32 accounts for the scaling introduced by the Scharr + // filter mask + cv::Scharr(*I, Ix, cv::DataType::type, 1, 0, 1. / 32.); + cv::Scharr(*I, Iy, cv::DataType::type, 0, 1, 1. / 32.); -PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, const ze::Camera::Ptr& camera) -{ - PointCloud pcl_camera; - for(const Event& ev : events) - { - Vector3 X_c = camera->backProject(ze::Keypoint(ev.x,ev.y)); - X_c[0] /= X_c[2]; - X_c[1] /= X_c[2]; - X_c[2] = 1.; - const ImageFloatType z = depthmap(ev.y,ev.x); - Vector3 P_c = z * X_c; - Vector3i rgb; - static const Vector3i red(255, 0, 0); - static const Vector3i blue(0, 0, 255); - rgb = (ev.pol) ? blue : red; - PointXYZRGB P_c_intensity(P_c, rgb); - pcl_camera.push_back(P_c_intensity); - } - return pcl_camera; -} + Image Lx, + Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I * + // dI/dx + static const ImageFloatType eps = + 1e-3; // small additive term to avoid problems at I=0 + cv::divide(Ix, *I + eps, Lx); + cv::divide(Iy, *I + eps, Ly); + + Image dLdt(height, width); + for (int y = 0; y < height; ++y) { + for (int x = 0; x < width; ++x) { + // dL/dt ~= - + const ImageFloatType dLdt_at_xy = + Lx(y, x) * (*flow)(y, x)[0] + + Ly(y, x) + * (*flow)( + y, + x + )[1]; // "-" sign ignored since we are + // interested in the absolute value... + dLdt(y, x) = std::fabs(dLdt_at_xy); + } + } + double min_dLdt, max_dLdt; + int min_idx, max_idx; + cv::minMaxIdx(dLdt, &min_dLdt, &max_dLdt, &min_idx, &max_idx); + + return static_cast(max_dLdt); + } + + void gaussianBlur(ImagePtr& I, FloatType sigma) { + cv::GaussianBlur(*I, *I, cv::Size(15, 15), sigma, sigma); + } + + CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera) { + CHECK(camera); + const ze::VectorX params = camera->projectionParameters(); + CalibrationMatrix K; + K << params(0), 0, params(2), 0, params(1), params(3), 0, 0, 1; + return K; + } + + PointCloud eventsToPointCloud( + const Events& events, + const Depthmap& depthmap, + const ze::Camera::Ptr& camera + ) { + PointCloud pcl_camera; + for (const Event& ev : events) { + Vector3 X_c = camera->backProject(ze::Keypoint(ev.x, ev.y)); + X_c[0] /= X_c[2]; + X_c[1] /= X_c[2]; + X_c[2] = 1.; + const ImageFloatType z = depthmap(ev.y, ev.x); + Vector3 P_c = z * X_c; + Vector3i rgb; + static const Vector3i red(255, 0, 0); + static const Vector3i blue(0, 0, 255); + rgb = (ev.pol) ? blue : red; + PointXYZRGB P_c_intensity(P_c, rgb); + pcl_camera.push_back(P_c_intensity); + } + return pcl_camera; + } } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_common/test/test_utils.cpp b/event_camera_simulator/esim_common/test/test_utils.cpp index 0f744d5..8809420 100644 --- a/event_camera_simulator/esim_common/test/test_utils.cpp +++ b/event_camera_simulator/esim_common/test/test_utils.cpp @@ -1,213 +1,252 @@ #include -#include +#include #include #include -#include -#include #include +#include +#include -std::string getTestDataDir(const std::string& dataset_name) -{ - using namespace ze; +std::string getTestDataDir(const std::string& dataset_name) { + using namespace ze; - const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH"); - CHECK(datapath_dir != nullptr) - << "Did you download the esim_test_data repository and set " - << "the ESIM_TEST_DATA_PATH environment variable?"; + const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH"); + CHECK(datapath_dir != nullptr) + << "Did you download the esim_test_data repository and set " + << "the ESIM_TEST_DATA_PATH environment variable?"; - std::string path(datapath_dir); - CHECK(isDir(path)) << "Folder does not exist: " << path; - path = path + "/data/" + dataset_name; - CHECK(isDir(path)) << "Dataset does not exist: " << path; - return path; + std::string path(datapath_dir); + CHECK(isDir(path)) << "Folder does not exist: " << path; + path = path + "/data/" + dataset_name; + CHECK(isDir(path)) << "Dataset does not exist: " << path; + return path; } namespace event_camera_simulator { -void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera, - const ze::Vector3& angular_velocity, - const ze::Vector3& linear_velocity, - const DepthmapPtr& depth, - OpticFlowPtr& flow) -{ - CHECK(flow); - CHECK_EQ(flow->rows, camera->height()); - CHECK_EQ(flow->cols, camera->width()); + void computeOpticFlowFiniteDifference( + const ze::Camera::Ptr& camera, + const ze::Vector3& angular_velocity, + const ze::Vector3& linear_velocity, + const DepthmapPtr& depth, + OpticFlowPtr& flow + ) { + CHECK(flow); + CHECK_EQ(flow->rows, camera->height()); + CHECK_EQ(flow->cols, camera->width()); - const FloatType dt = 0.001; + const FloatType dt = 0.001; - for(int y=0; yrows; ++y) - { - for(int x=0; xcols; ++x) - { - ze::Keypoint u_t(x,y); - ze::Bearing f = camera->backProject(u_t); - const FloatType z = static_cast((*depth)(y,x)); - ze::Position Xc_t = z * ze::Position(f[0]/f[2], f[1]/f[2], 1.); + for (int y = 0; y < flow->rows; ++y) { + for (int x = 0; x < flow->cols; ++x) { + ze::Keypoint u_t(x, y); + ze::Bearing f = camera->backProject(u_t); + const FloatType z = static_cast((*depth)(y, x)); + ze::Position Xc_t = + z * ze::Position(f[0] / f[2], f[1] / f[2], 1.); - ze::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt); - ze::Transformation::Position dT = -linear_velocity * dt; + ze::Transformation::Rotation dR = + ze::Transformation::Rotation::exp(-angular_velocity * dt); + ze::Transformation::Position dT = -linear_velocity * dt; - // Transform Xc(t) to Xc(t+dt) - ze::Transformation T_tdt_t; - T_tdt_t.getRotation() = dR; - T_tdt_t.getPosition() = dT; - VLOG(5) << T_tdt_t; + // Transform Xc(t) to Xc(t+dt) + ze::Transformation T_tdt_t; + T_tdt_t.getRotation() = dR; + T_tdt_t.getPosition() = dT; + VLOG(5) << T_tdt_t; - ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t); + ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t); - // Project Xc(t+dt) in the image plane - ze::Keypoint u_t_dt = camera->project(Xc_t_dt); - VLOG(5) << u_t; - VLOG(5) << u_t_dt; + // Project Xc(t+dt) in the image plane + ze::Keypoint u_t_dt = camera->project(Xc_t_dt); + VLOG(5) << u_t; + VLOG(5) << u_t_dt; - ze::Vector2 flow_vec = (u_t_dt - u_t) / dt; - (*flow)(y,x) = cv::Vec(flow_vec(0), flow_vec(1)); + ze::Vector2 flow_vec = (u_t_dt - u_t) / dt; + (*flow)(y, x) = cv::Vec(flow_vec(0), flow_vec(1)); + } + } } - } -} -void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera, - const ze::Vector3& angular_velocity, - const ze::Vector3& linear_velocity, - const DepthmapPtr& depth, - const ze::Vector3& r_COBJ, - const ze::Vector3& angular_velocity_obj, - const ze::Vector3& linear_velocity_obj, - OpticFlowPtr& flow) -{ - CHECK(flow); - CHECK_EQ(flow->rows, camera->height()); - CHECK_EQ(flow->cols, camera->width()); + void computeOpticFlowFiniteDifference( + const ze::Camera::Ptr& camera, + const ze::Vector3& angular_velocity, + const ze::Vector3& linear_velocity, + const DepthmapPtr& depth, + const ze::Vector3& r_COBJ, + const ze::Vector3& angular_velocity_obj, + const ze::Vector3& linear_velocity_obj, + OpticFlowPtr& flow + ) { + CHECK(flow); + CHECK_EQ(flow->rows, camera->height()); + CHECK_EQ(flow->cols, camera->width()); - const FloatType dt = 0.001; + const FloatType dt = 0.001; - for(int y=0; yrows; ++y) - { - for(int x=0; xcols; ++x) - { - ze::Keypoint u_t(x,y); - ze::Bearing f = camera->backProject(u_t); - const FloatType z = static_cast((*depth)(y,x)); - ze::Position Xc_t = z * ze::Position(f[0]/f[2], f[1]/f[2], 1.); - ze::Position r_OBJX = Xc_t - r_COBJ; - ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(angular_velocity_obj); + for (int y = 0; y < flow->rows; ++y) { + for (int x = 0; x < flow->cols; ++x) { + ze::Keypoint u_t(x, y); + ze::Bearing f = camera->backProject(u_t); + const FloatType z = static_cast((*depth)(y, x)); + ze::Position Xc_t = + z * ze::Position(f[0] / f[2], f[1] / f[2], 1.); + ze::Position r_OBJX = Xc_t - r_COBJ; + ze::Matrix33 w_WOBJ_tilde = + ze::skewSymmetric(angular_velocity_obj); - ze::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt); - ze::Transformation::Position dT = linear_velocity_obj*dt - linear_velocity * dt + w_WOBJ_tilde*r_OBJX*dt; + ze::Transformation::Rotation dR = + ze::Transformation::Rotation::exp(-angular_velocity * dt); + ze::Transformation::Position dT = linear_velocity_obj * dt + - linear_velocity * dt + + w_WOBJ_tilde * r_OBJX * dt; - // Transform Xc(t) to Xc(t+dt) - ze::Transformation T_tdt_t; - T_tdt_t.getRotation() = dR; - T_tdt_t.getPosition() = dT; - VLOG(5) << T_tdt_t; + // Transform Xc(t) to Xc(t+dt) + ze::Transformation T_tdt_t; + T_tdt_t.getRotation() = dR; + T_tdt_t.getPosition() = dT; + VLOG(5) << T_tdt_t; - ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t); + ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t); - // Project Xc(t+dt) in the image plane - ze::Keypoint u_t_dt = camera->project(Xc_t_dt); - VLOG(5) << u_t; - VLOG(5) << u_t_dt; + // Project Xc(t+dt) in the image plane + ze::Keypoint u_t_dt = camera->project(Xc_t_dt); + VLOG(5) << u_t; + VLOG(5) << u_t_dt; - ze::Vector2 flow_vec = (u_t_dt - u_t) / dt; - (*flow)(y,x) = cv::Vec(flow_vec(0), flow_vec(1)); + ze::Vector2 flow_vec = (u_t_dt - u_t) / dt; + (*flow)(y, x) = cv::Vec(flow_vec(0), flow_vec(1)); + } + } } - } -} -} // event_camera_simulator +} // namespace event_camera_simulator -TEST(Utils, testOpticFlowComputation) -{ - using namespace event_camera_simulator; +TEST(Utils, testOpticFlowComputation) { + using namespace event_camera_simulator; - // Load camera calib from folder - const std::string path_to_data_folder = getTestDataDir("camera_calibs"); - ze::CameraRig::Ptr camera_rig - = ze::cameraRigFromYaml(ze::joinPath(path_to_data_folder, "pinhole_mono.yaml")); + // Load camera calib from folder + const std::string path_to_data_folder = getTestDataDir("camera_calibs"); + ze::CameraRig::Ptr camera_rig = ze::cameraRigFromYaml( + ze::joinPath(path_to_data_folder, "pinhole_mono.yaml") + ); - CHECK(camera_rig); + CHECK(camera_rig); - const ze::Camera::Ptr camera = camera_rig->atShared(0); - cv::Size sensor_size(camera->width(), camera->height()); + const ze::Camera::Ptr camera = camera_rig->atShared(0); + cv::Size sensor_size(camera->width(), camera->height()); - OpticFlowPtr flow_analytic = - std::make_shared(sensor_size); + OpticFlowPtr flow_analytic = std::make_shared(sensor_size); - // Sample random depth map - const ImageFloatType z_mean = 5.0; - const ImageFloatType z_stddev = 0.5; - DepthmapPtr depth = std::make_shared(sensor_size); - for(int y=0; y(sensor_size); + for (int y = 0; y < sensor_size.height; ++y) { + for (int x = 0; x < sensor_size.width; ++x) { + (*depth)(y, x) = + ze::sampleNormalDistribution(true, z_mean, z_stddev); + } } - } - // Sample random linear and angular velocity - ze::Vector3 angular_velocity, linear_velocity; - angular_velocity.setRandom(); - linear_velocity.setRandom(); + // Sample random linear and angular velocity + ze::Vector3 angular_velocity, linear_velocity; + angular_velocity.setRandom(); + linear_velocity.setRandom(); - LOG(INFO) << "w = " << angular_velocity; - LOG(INFO) << "v = " << linear_velocity; + LOG(INFO) << "w = " << angular_velocity; + LOG(INFO) << "v = " << linear_velocity; - // Compute optic flow on the image plane according - // to the sampled angular/linear velocity - OpticFlowHelper optic_flow_helper(camera); - optic_flow_helper.computeFromDepthAndTwist(angular_velocity, linear_velocity, - depth, flow_analytic); + // Compute optic flow on the image plane according + // to the sampled angular/linear velocity + OpticFlowHelper optic_flow_helper(camera); + optic_flow_helper.computeFromDepthAndTwist( + angular_velocity, + linear_velocity, + depth, + flow_analytic + ); - OpticFlowPtr flow_finite_diff = - std::make_shared(sensor_size); + OpticFlowPtr flow_finite_diff = std::make_shared(sensor_size); - computeOpticFlowFiniteDifference(camera, angular_velocity, linear_velocity, - depth, flow_finite_diff); + computeOpticFlowFiniteDifference( + camera, + angular_velocity, + linear_velocity, + depth, + flow_finite_diff + ); - // Check that the analytical flow and finite-difference flow match - for(int y=0; y +#include #include #include - -#include #include #include #include // fwd namespace cv { -class Mat; + class Mat; } namespace event_camera_simulator { -using Callback = - std::function; + using Callback = std::function; -enum class DataProviderType { - RendererOnline, - Folder, - Rosbag -}; + enum class DataProviderType { RendererOnline, Folder, Rosbag }; -//! A data provider registers to a data source and triggers callbacks when -//! new data is available. -class DataProviderBase : ze::Noncopyable -{ -public: - ZE_POINTER_TYPEDEFS(DataProviderBase); + //! A data provider registers to a data source and triggers callbacks when + //! new data is available. + class DataProviderBase : ze::Noncopyable { + public: + ZE_POINTER_TYPEDEFS(DataProviderBase); - DataProviderBase() = delete; - DataProviderBase(DataProviderType type); - virtual ~DataProviderBase() = default; + DataProviderBase() = delete; + DataProviderBase(DataProviderType type); + virtual ~DataProviderBase() = default; - //! Process all callbacks. Waits until callback is processed. - void spin(); + //! Process all callbacks. Waits until callback is processed. + void spin(); - //! Read next data field and process callback. Returns false when datatset finished. - virtual bool spinOnce() = 0; + //! Read next data field and process callback. Returns false when + //! datatset finished. + virtual bool spinOnce() = 0; - //! False if there is no more data to process or there was a shutdown signal. - virtual bool ok() const = 0; + //! False if there is no more data to process or there was a shutdown + //! signal. + virtual bool ok() const = 0; - //! Pause data provider. - virtual void pause(); + //! Pause data provider. + virtual void pause(); - //! Stop data provider. - virtual void shutdown(); + //! Stop data provider. + virtual void shutdown(); - //! Register callback function to call when new message is available. - void registerCallback(const Callback& callback); + //! Register callback function to call when new message is available. + void registerCallback(const Callback& callback); - //! Returns the number of cameras in the rig - virtual size_t numCameras() const = 0; + //! Returns the number of cameras in the rig + virtual size_t numCameras() const = 0; - //! Returns the camera rig - ze::CameraRig::Ptr getCameraRig() { return camera_rig_; } + //! Returns the camera rig + ze::CameraRig::Ptr getCameraRig() { + return camera_rig_; + } -protected: - DataProviderType type_; - Callback callback_; - volatile bool running_ = true; + protected: + DataProviderType type_; + Callback callback_; + volatile bool running_ = true; - ze::CameraRig::Ptr camera_rig_; + ze::CameraRig::Ptr camera_rig_; -private: - ze::SimpleSigtermHandler signal_handler_; //!< Sets running_ to false when Ctrl-C is pressed. -}; + private: + ze::SimpleSigtermHandler + signal_handler_; //!< Sets running_ to false when Ctrl-C is pressed. + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp index e69b8ef..4c848a2 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp @@ -1,10 +1,10 @@ #pragma once -#include #include +#include namespace event_camera_simulator { -DataProviderBase::Ptr loadDataProviderFromGflags(); + DataProviderBase::Ptr loadDataProviderFromGflags(); -} // namespace ze +} // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp index 1dbe0d3..6c2765c 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp @@ -1,44 +1,41 @@ #pragma once +#include +#include #include #include #include #include - +#include #include #include -#include -#include -#include namespace event_camera_simulator { -class DataProviderFromFolder : public DataProviderBase -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + class DataProviderFromFolder : public DataProviderBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - DataProviderFromFolder(const std::string& path_to_data_folder); + DataProviderFromFolder(const std::string& path_to_data_folder); - virtual ~DataProviderFromFolder() = default; + virtual ~DataProviderFromFolder() = default; - virtual bool spinOnce() override; + virtual bool spinOnce() override; - virtual bool ok() const override; + virtual bool ok() const override; - size_t numCameras() const override; + size_t numCameras() const override; -private: + private: + int64_t getTimeStamp(const std::string& ts_str) const; - int64_t getTimeStamp(const std::string& ts_str) const; + std::string path_to_data_folder_; + std::ifstream images_in_str_; + const char delimiter_{','}; + const size_t num_tokens_in_line_ = 2; // stamp, image + bool finished_parsing_; - std::string path_to_data_folder_; - std::ifstream images_in_str_; - const char delimiter_{','}; - const size_t num_tokens_in_line_ = 2; // stamp, image - bool finished_parsing_; - - SimulatorData sim_data_; -}; + SimulatorData sim_data_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp index 57d3c38..d0d4ed7 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp @@ -1,75 +1,77 @@ #pragma once +#include +#include +#include #include #include #include #include - +#include #include #include -#include -#include -#include - -#include #include -#include - +#include namespace event_camera_simulator { -/** - * Online data provider intended to simulate a camera rig composed of multiple - * cameras rigidly attached together, along with an Inertial Measurement Unit (IMU). - * - * The camera rig follows a camera trajectory in 3D. - */ -class DataProviderOnlineMoving3DCameraRig : public DataProviderBase -{ -public: - DataProviderOnlineMoving3DCameraRig(ze::real_t simulation_minimum_framerate, - ze::real_t simulation_imu_rate, - int simulation_adaptive_sampling_method, - ze::real_t simulation_adaptive_sampling_lambda); + /** + * Online data provider intended to simulate a camera rig composed of + * multiple cameras rigidly attached together, along with an Inertial + * Measurement Unit (IMU). + * + * The camera rig follows a camera trajectory in 3D. + */ + class DataProviderOnlineMoving3DCameraRig : public DataProviderBase { + public: + DataProviderOnlineMoving3DCameraRig( + ze::real_t simulation_minimum_framerate, + ze::real_t simulation_imu_rate, + int simulation_adaptive_sampling_method, + ze::real_t simulation_adaptive_sampling_lambda + ); - virtual ~DataProviderOnlineMoving3DCameraRig(); + virtual ~DataProviderOnlineMoving3DCameraRig(); - virtual bool spinOnce() override; + virtual bool spinOnce() override; - virtual bool ok() const override; + virtual bool ok() const override; - size_t numCameras() const override; + size_t numCameras() const override; -private: + private: + void updateGroundtruth(); + void sampleImu(); + void sampleFrame(); - void updateGroundtruth(); - void sampleImu(); - void sampleFrame(); + void setImuUpdated(); + void setFrameUpdated(); + void setAllUpdated(); - void setImuUpdated(); - void setFrameUpdated(); - void setAllUpdated(); + std::vector renderers_; + std::vector optic_flow_helpers_; + ze::TrajectorySimulator::Ptr trajectory_; + ze::ImuSimulator::Ptr imu_; + SimulatorData sim_data_; - std::vector renderers_; - std::vector optic_flow_helpers_; - ze::TrajectorySimulator::Ptr trajectory_; - ze::ImuSimulator::Ptr imu_; - SimulatorData sim_data_; + ze::real_t t_; + ze::real_t last_t_frame_; // latest next sampling time in order to + // guarantee the IMU rate + ze::real_t last_t_imu_; // latest next sampling time in order to + // guarantee the minimum frame rate + ze::real_t next_t_flow_; // latest next sampling time in order to + // guarantee that the max pixel displacement + // since the last frame is bounded + ze::real_t dt_imu_; + ze::real_t dt_frame_; - ze::real_t t_; - ze::real_t last_t_frame_; // latest next sampling time in order to guarantee the IMU rate - ze::real_t last_t_imu_; // latest next sampling time in order to guarantee the minimum frame rate - ze::real_t next_t_flow_; // latest next sampling time in order to guarantee that the max pixel displacement since the last frame is bounded - ze::real_t dt_imu_; - ze::real_t dt_frame_; + ze::real_t simulation_minimum_framerate_; + ze::real_t simulation_imu_rate_; + int simulation_adaptive_sampling_method_; + ze::real_t simulation_adaptive_sampling_lambda_; - ze::real_t simulation_minimum_framerate_; - ze::real_t simulation_imu_rate_; - int simulation_adaptive_sampling_method_; - ze::real_t simulation_adaptive_sampling_lambda_; - - // dynamic objects - std::vector trajectory_dyn_obj_; -}; + // dynamic objects + std::vector trajectory_dyn_obj_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp index fc92e04..a5ef1d1 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp @@ -1,54 +1,56 @@ #pragma once +#include +#include #include #include #include #include - #include #include -#include -#include - namespace event_camera_simulator { -/** - * Simple online data provider, intended to simulate a single event camera - * based on images + optic flow maps provided by a rendering engine. - * - * This data provider does NOT simulate a camera trajectory or an IMU. - */ -class DataProviderOnlineSimple : public DataProviderBase -{ -public: - DataProviderOnlineSimple(ze::real_t simulation_minimum_framerate, - int simulation_adaptive_sampling_method, - ze::real_t simulation_adaptive_sampling_lambda); + /** + * Simple online data provider, intended to simulate a single event camera + * based on images + optic flow maps provided by a rendering engine. + * + * This data provider does NOT simulate a camera trajectory or an IMU. + */ + class DataProviderOnlineSimple : public DataProviderBase { + public: + DataProviderOnlineSimple( + ze::real_t simulation_minimum_framerate, + int simulation_adaptive_sampling_method, + ze::real_t simulation_adaptive_sampling_lambda + ); - virtual ~DataProviderOnlineSimple(); + virtual ~DataProviderOnlineSimple(); - virtual bool spinOnce() override; + virtual bool spinOnce() override; - virtual bool ok() const override; + virtual bool ok() const override; - size_t numCameras() const override; + size_t numCameras() const override; -private: - bool sampleFrame(); - void setFrameUpdated(); + private: + bool sampleFrame(); + void setFrameUpdated(); - SimpleRenderer::Ptr renderer_; - SimulatorData sim_data_; + SimpleRenderer::Ptr renderer_; + SimulatorData sim_data_; - ze::real_t t_; - ze::real_t last_t_frame_; // latest next sampling time in order to guarantee the minimum frame rate - ze::real_t next_t_adaptive_; // latest next sampling time in order to guarantee that the adaptive sampling scheme is respected - ze::real_t dt_frame_; + ze::real_t t_; + ze::real_t last_t_frame_; // latest next sampling time in order to + // guarantee the minimum frame rate + ze::real_t + next_t_adaptive_; // latest next sampling time in order to guarantee + // that the adaptive sampling scheme is respected + ze::real_t dt_frame_; - ze::real_t simulation_minimum_framerate_; - int simulation_adaptive_sampling_method_; - ze::real_t simulation_adaptive_sampling_lambda_; -}; + ze::real_t simulation_minimum_framerate_; + int simulation_adaptive_sampling_method_; + ze::real_t simulation_adaptive_sampling_lambda_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp index 9f8efb3..eb5ec11 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp @@ -1,57 +1,57 @@ -// This file was adapted from the ze_oss project: https://github.com/zurich-eye/ze_oss/blob/master/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp +// This file was adapted from the ze_oss project: +// https://github.com/zurich-eye/ze_oss/blob/master/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp // Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved #pragma once +#include +#include #include -#include #include -#include - -#include - #include #include - #include - -#include +#include +#include namespace event_camera_simulator { -class DataProviderRosbag : public DataProviderBase -{ -public: - DataProviderRosbag( - const std::string& bag_filename, - const std::map& camera_topics); + class DataProviderRosbag : public DataProviderBase { + public: + DataProviderRosbag( + const std::string& bag_filename, + const std::map& camera_topics + ); - virtual ~DataProviderRosbag() = default; + virtual ~DataProviderRosbag() = default; - virtual bool spinOnce() override; + virtual bool spinOnce() override; - virtual bool ok() const override; + virtual bool ok() const override; - virtual size_t numCameras() const; + virtual size_t numCameras() const; - size_t size() const; + size_t size() const; -private: - void loadRosbag(const std::string& bag_filename); - void initBagView(const std::vector& topics); + private: + void loadRosbag(const std::string& bag_filename); + void initBagView(const std::vector& topics); - inline bool cameraSpin(const sensor_msgs::ImageConstPtr m_img, - const rosbag::MessageInstance& m); + inline bool cameraSpin( + const sensor_msgs::ImageConstPtr m_img, + const rosbag::MessageInstance& m + ); - std::unique_ptr bag_; - std::unique_ptr bag_view_; - rosbag::View::iterator bag_view_it_; - int n_processed_images_ = 0; + std::unique_ptr bag_; + std::unique_ptr bag_view_; + rosbag::View::iterator bag_view_it_; + int n_processed_images_ = 0; - // subscribed topics: - std::map img_topic_camidx_map_; // camera_topic --> camera_id + // subscribed topics: + std::map + img_topic_camidx_map_; // camera_topic --> camera_id - SimulatorData sim_data_; -}; + SimulatorData sim_data_; + }; } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp b/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp index 88fe073..fc16600 100644 --- a/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp +++ b/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp @@ -1,13 +1,13 @@ #pragma once -#include #include #include +#include namespace event_camera_simulator { -bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat *img); -Renderer::Ptr loadRendererFromGflags(); -SimpleRenderer::Ptr loadSimpleRendererFromGflags(); + bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img); + Renderer::Ptr loadRendererFromGflags(); + SimpleRenderer::Ptr loadSimpleRendererFromGflags(); } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp b/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp index 0387394..d3e5f04 100644 --- a/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp +++ b/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp @@ -2,32 +2,25 @@ namespace event_camera_simulator { -DataProviderBase::DataProviderBase(DataProviderType type) - : type_(type) - , signal_handler_(running_) -{} + DataProviderBase::DataProviderBase(DataProviderType type) + : type_(type), + signal_handler_(running_) {} -void DataProviderBase::spin() -{ - while (ok()) - { - spinOnce(); - } -} + void DataProviderBase::spin() { + while (ok()) + spinOnce(); + } -void DataProviderBase::pause() -{ - running_ = false; -} + void DataProviderBase::pause() { + running_ = false; + } -void DataProviderBase::shutdown() -{ - running_ = false; -} + void DataProviderBase::shutdown() { + running_ = false; + } -void DataProviderBase::registerCallback(const Callback& callback) -{ - callback_ = callback; -} + void DataProviderBase::registerCallback(const Callback& callback) { + callback_ = callback; + } } // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp b/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp index f283d78..884da21 100644 --- a/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp +++ b/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp @@ -1,38 +1,50 @@ -#include -#include #include +#include +#include #include #include -#include #include +#include DEFINE_int32(data_source, 0, " 0: Online renderer"); -DEFINE_double(simulation_minimum_framerate, 30.0, - "Minimum frame rate, in Hz" - "Especially useful when the event rate is low, to guarantee" - "that frames are still published at a minimum framerate"); +DEFINE_double( + simulation_minimum_framerate, + 30.0, + "Minimum frame rate, in Hz" + "Especially useful when the event rate is low, to guarantee" + "that frames are still published at a minimum framerate" +); -DEFINE_double(simulation_imu_rate, 1000.0, - "Fixed IMU sampling frequency, in Hz"); +DEFINE_double( + simulation_imu_rate, 1000.0, "Fixed IMU sampling frequency, in Hz" +); -DEFINE_int32(simulation_adaptive_sampling_method, 0, - "Method to use for adaptive sampling." - "0: based on predicted absolute brightness change" - "1: based on optic flow"); +DEFINE_int32( + simulation_adaptive_sampling_method, + 0, + "Method to use for adaptive sampling." + "0: based on predicted absolute brightness change" + "1: based on optic flow" +); -DEFINE_double(simulation_adaptive_sampling_lambda, 0.5, - "Parameter that controls the behavior of the adaptive sampling method." - "The meaning of this value depends on the adaptive sampling method used:" - "...based on predicted absolute brightness change: deltaT = lambda / max(|dL/dt|)" - "...based on optic flow: deltaT = lambda \ max(||du/dt||) where du/dt denotes the 2D optic flow field."); - -DEFINE_string(path_to_data_folder, "", - "Path to folder containing the data."); +DEFINE_double( + simulation_adaptive_sampling_lambda, + 0.5, + "Parameter that controls the behavior of the adaptive sampling method." + "The meaning of this value depends on the adaptive sampling method used:" + "...based on predicted absolute brightness change: deltaT = lambda / " + "max(|dL/dt|)" + "...based on optic flow: deltaT = lambda \ max(||du/dt||) where du/dt " + "denotes the 2D optic flow field." +); +DEFINE_string(path_to_data_folder, "", "Path to folder containing the data."); // Parameters for the DataProviderRosbag -DEFINE_string(bag_filename, "dataset.bag", "Name of bagfile from which to read."); +DEFINE_string( + bag_filename, "dataset.bag", "Name of bagfile from which to read." +); DEFINE_string(topic_cam0, "/cam0/image_raw", ""); DEFINE_string(topic_cam1, "/cam1/image_raw", ""); DEFINE_string(topic_cam2, "/cam2/image_raw", ""); @@ -41,59 +53,64 @@ DEFINE_uint64(num_cams, 1, "Number of normal cameras to read from rosbag."); namespace event_camera_simulator { -DataProviderBase::Ptr loadDataProviderFromGflags() -{ - // Create data provider. - DataProviderBase::Ptr data_provider; - switch (FLAGS_data_source) - { - case 0: // Online Renderer for Moving 3D Camera Rig with IMU - { - data_provider.reset( - new DataProviderOnlineMoving3DCameraRig(FLAGS_simulation_minimum_framerate, - FLAGS_simulation_imu_rate, - FLAGS_simulation_adaptive_sampling_method, - FLAGS_simulation_adaptive_sampling_lambda)); - break; - } - case 1: // Online Renderer Simple - { - data_provider.reset( - new DataProviderOnlineSimple(FLAGS_simulation_minimum_framerate, - FLAGS_simulation_adaptive_sampling_method, - FLAGS_simulation_adaptive_sampling_lambda)); - break; - } - case 2: // Read data from a folder - { - data_provider.reset( - new DataProviderFromFolder(FLAGS_path_to_data_folder)); - break; - } - case 3: // Read data (images) from a rosbag - { - CHECK_LE(FLAGS_num_cams, 4u); - CHECK_EQ(FLAGS_num_cams, 1u) << "Only one camera is supported currently"; + DataProviderBase::Ptr loadDataProviderFromGflags() { + // Create data provider. + DataProviderBase::Ptr data_provider; + switch (FLAGS_data_source) { + case 0: // Online Renderer for Moving 3D Camera Rig with IMU + { + data_provider.reset(new DataProviderOnlineMoving3DCameraRig( + FLAGS_simulation_minimum_framerate, + FLAGS_simulation_imu_rate, + FLAGS_simulation_adaptive_sampling_method, + FLAGS_simulation_adaptive_sampling_lambda + )); + break; + } + case 1: // Online Renderer Simple + { + data_provider.reset(new DataProviderOnlineSimple( + FLAGS_simulation_minimum_framerate, + FLAGS_simulation_adaptive_sampling_method, + FLAGS_simulation_adaptive_sampling_lambda + )); + break; + } + case 2: // Read data from a folder + { + data_provider.reset( + new DataProviderFromFolder(FLAGS_path_to_data_folder) + ); + break; + } + case 3: // Read data (images) from a rosbag + { + CHECK_LE(FLAGS_num_cams, 4u); + CHECK_EQ(FLAGS_num_cams, 1u) + << "Only one camera is supported currently"; - // Fill camera topics. - std::map cam_topics; - if (FLAGS_num_cams >= 1) cam_topics[FLAGS_topic_cam0] = 0; - if (FLAGS_num_cams >= 2) cam_topics[FLAGS_topic_cam1] = 1; - if (FLAGS_num_cams >= 3) cam_topics[FLAGS_topic_cam2] = 2; - if (FLAGS_num_cams >= 4) cam_topics[FLAGS_topic_cam3] = 3; - data_provider.reset( - new DataProviderRosbag(FLAGS_bag_filename, - cam_topics)); - break; - } - default: - { - LOG(FATAL) << "Data source not known."; - break; - } - } + // Fill camera topics. + std::map cam_topics; + if (FLAGS_num_cams >= 1) + cam_topics[FLAGS_topic_cam0] = 0; + if (FLAGS_num_cams >= 2) + cam_topics[FLAGS_topic_cam1] = 1; + if (FLAGS_num_cams >= 3) + cam_topics[FLAGS_topic_cam2] = 2; + if (FLAGS_num_cams >= 4) + cam_topics[FLAGS_topic_cam3] = 3; + data_provider.reset( + new DataProviderRosbag(FLAGS_bag_filename, cam_topics) + ); + break; + } + default: { + LOG(FATAL) << "Data source not known."; + break; + } + } - return data_provider; -} + return data_provider; + } -} // namespace ze +} // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp b/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp index fb61a31..3e5258a 100644 --- a/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp +++ b/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp @@ -1,92 +1,105 @@ #include -#include #include #include +#include namespace event_camera_simulator { -DataProviderFromFolder::DataProviderFromFolder(const std::string& path_to_data_folder) - : DataProviderBase(DataProviderType::Folder), - path_to_data_folder_(path_to_data_folder), - finished_parsing_(false) -{ - // Load CSV image file - images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv")); - CHECK(images_in_str_.is_open()); + DataProviderFromFolder::DataProviderFromFolder( + const std::string& path_to_data_folder + ) + : DataProviderBase(DataProviderType::Folder), + path_to_data_folder_(path_to_data_folder), + finished_parsing_(false) { + // Load CSV image file + images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv")); + CHECK(images_in_str_.is_open()); - // Create dummy camera rig - // these intrinsic values are filled with garbage (width = height = 1) since the actual values are not known - ze::CameraVector cameras; - cameras.emplace_back(ze::createEquidistantCameraShared(1, 1, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - ze::TransformationVector extrinsics; - extrinsics.push_back(ze::Transformation()); - camera_rig_ = std::make_shared(extrinsics, cameras, "camera"); + // Create dummy camera rig + // these intrinsic values are filled with garbage (width = height = 1) + // since the actual values are not known + ze::CameraVector cameras; + cameras.emplace_back(ze::createEquidistantCameraShared( + 1, + 1, + 1.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + )); + ze::TransformationVector extrinsics; + extrinsics.push_back(ze::Transformation()); + camera_rig_ = + std::make_shared(extrinsics, cameras, "camera"); - // Allocate memory for image data - sim_data_.images.emplace_back(ImagePtr(new Image( - cv::Size(camera_rig_->at(0).width(), - camera_rig_->at(0).height())))); + // Allocate memory for image data + sim_data_.images.emplace_back(ImagePtr(new Image( + cv::Size(camera_rig_->at(0).width(), camera_rig_->at(0).height()) + ))); - sim_data_.camera_rig = camera_rig_; - sim_data_.images_updated = true; - sim_data_.depthmaps_updated = false; - sim_data_.optic_flows_updated = false; - sim_data_.twists_updated = false; - sim_data_.poses_updated = false; - sim_data_.imu_updated = false; -} - -int64_t DataProviderFromFolder::getTimeStamp(const std::string& ts_str) const -{ - return std::stoll(ts_str); -} - -size_t DataProviderFromFolder::numCameras() const -{ - return camera_rig_->size(); -} - -bool DataProviderFromFolder::spinOnce() -{ - std::string line; - if(!getline(images_in_str_, line)) - { - VLOG(1) << "Finished parsing images.csv file"; - finished_parsing_ = true; - return false; - } - - if('%' != line.at(0) && '#' != line.at(0)) - { - std::vector items = ze::splitString(line, delimiter_); - CHECK_GE(items.size(), num_tokens_in_line_); - int64_t stamp = getTimeStamp(items[0]); - - const std::string& path_to_image = ze::joinPath(path_to_data_folder_, items[1]); - cv::Mat image = cv::imread(path_to_image, 0); - CHECK(image.data) << "Could not load image from file: " << path_to_image; - - VLOG(3) << "Read image from file: " << path_to_image; - image.convertTo(*sim_data_.images[0], cv::DataType::type, 1./255.); - - if(callback_) - { - sim_data_.timestamp = static_cast