mirror of
https://github.com/karma-riuk/hdr_esim.git
synced 2025-06-19 10:57:51 +02:00
initial commit
This commit is contained in:
32
event_camera_simulator/esim_data_provider/CMakeLists.txt
Normal file
32
event_camera_simulator/esim_data_provider/CMakeLists.txt
Normal file
@ -0,0 +1,32 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(esim_data_provider)
|
||||
|
||||
find_package(catkin_simple REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
catkin_simple()
|
||||
|
||||
set(HEADERS
|
||||
include/esim/data_provider/data_provider_base.hpp
|
||||
include/esim/data_provider/data_provider_factory.hpp
|
||||
include/esim/data_provider/data_provider_online_render.hpp
|
||||
include/esim/data_provider/data_provider_online_simple.hpp
|
||||
include/esim/data_provider/data_provider_from_folder.hpp
|
||||
include/esim/data_provider/data_provider_rosbag.hpp
|
||||
include/esim/data_provider/renderer_factory.hpp
|
||||
)
|
||||
|
||||
set(SOURCES
|
||||
src/data_provider_base.cpp
|
||||
src/data_provider_factory.cpp
|
||||
src/data_provider_online_render.cpp
|
||||
src/data_provider_online_simple.cpp
|
||||
src/data_provider_from_folder.cpp
|
||||
src/data_provider_rosbag.cpp
|
||||
src/renderer_factory.cpp
|
||||
)
|
||||
|
||||
cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
|
||||
|
||||
cs_install()
|
||||
cs_export()
|
@ -0,0 +1,74 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <esim/common/types.hpp>
|
||||
#include <ze/common/macros.hpp>
|
||||
#include <ze/common/noncopyable.hpp>
|
||||
#include <ze/common/signal_handler.hpp>
|
||||
|
||||
// fwd
|
||||
namespace cv {
|
||||
class Mat;
|
||||
}
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
using Callback =
|
||||
std::function<void (const SimulatorData& sim_data)>;
|
||||
|
||||
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);
|
||||
|
||||
DataProviderBase() = delete;
|
||||
DataProviderBase(DataProviderType type);
|
||||
virtual ~DataProviderBase() = default;
|
||||
|
||||
//! 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;
|
||||
|
||||
//! 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();
|
||||
|
||||
//! Stop data provider.
|
||||
virtual void shutdown();
|
||||
|
||||
//! 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 camera rig
|
||||
ze::CameraRig::Ptr getCameraRig() { return camera_rig_; }
|
||||
|
||||
protected:
|
||||
DataProviderType type_;
|
||||
Callback callback_;
|
||||
volatile bool running_ = true;
|
||||
|
||||
ze::CameraRig::Ptr camera_rig_;
|
||||
|
||||
private:
|
||||
ze::SimpleSigtermHandler signal_handler_; //!< Sets running_ to false when Ctrl-C is pressed.
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderBase::Ptr loadDataProviderFromGflags();
|
||||
|
||||
} // namespace ze
|
@ -0,0 +1,43 @@
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <ze/common/macros.hpp>
|
||||
#include <ze/common/types.hpp>
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
#include <ze/cameras/camera_rig.hpp>
|
||||
#include <fstream>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class DataProviderFromFolder : public DataProviderBase
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
DataProviderFromFolder(const std::string& path_to_data_folder);
|
||||
|
||||
virtual ~DataProviderFromFolder() = default;
|
||||
|
||||
virtual bool spinOnce() override;
|
||||
|
||||
virtual bool ok() const override;
|
||||
|
||||
size_t numCameras() const override;
|
||||
|
||||
private:
|
||||
|
||||
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_ = 3; // stamp, image, depth
|
||||
|
||||
SimulatorData sim_data_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,75 @@
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <ze/common/macros.hpp>
|
||||
#include <ze/common/types.hpp>
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
#include <esim/rendering/renderer_base.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
|
||||
#include <ze/vi_simulation/trajectory_simulator.hpp>
|
||||
#include <ze/vi_simulation/imu_simulator.hpp>
|
||||
#include <ze/cameras/camera_rig.hpp>
|
||||
|
||||
|
||||
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);
|
||||
|
||||
virtual ~DataProviderOnlineMoving3DCameraRig();
|
||||
|
||||
virtual bool spinOnce() override;
|
||||
|
||||
virtual bool ok() const override;
|
||||
|
||||
size_t numCameras() const override;
|
||||
|
||||
private:
|
||||
|
||||
void updateGroundtruth();
|
||||
void sampleImu();
|
||||
void sampleFrame();
|
||||
|
||||
void setImuUpdated();
|
||||
void setFrameUpdated();
|
||||
void setAllUpdated();
|
||||
|
||||
std::vector<Renderer::Ptr> renderers_;
|
||||
std::vector<OpticFlowHelper::Ptr> 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 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<ze::TrajectorySimulator::Ptr> trajectory_dyn_obj_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,54 @@
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <ze/common/macros.hpp>
|
||||
#include <ze/common/types.hpp>
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
#include <esim/rendering/simple_renderer_base.hpp>
|
||||
|
||||
|
||||
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);
|
||||
|
||||
virtual ~DataProviderOnlineSimple();
|
||||
|
||||
virtual bool spinOnce() override;
|
||||
|
||||
virtual bool ok() const override;
|
||||
|
||||
size_t numCameras() const override;
|
||||
|
||||
private:
|
||||
bool sampleFrame();
|
||||
void setFrameUpdated();
|
||||
|
||||
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 simulation_minimum_framerate_;
|
||||
int simulation_adaptive_sampling_method_;
|
||||
ze::real_t simulation_adaptive_sampling_lambda_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,56 @@
|
||||
// Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include<gflags/gflags.h>
|
||||
|
||||
#include <rosbag/bag.h>
|
||||
#include <rosbag/view.h>
|
||||
|
||||
#include <sensor_msgs/Image.h>
|
||||
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class DataProviderRosbag : public DataProviderBase
|
||||
{
|
||||
public:
|
||||
DataProviderRosbag(
|
||||
const std::string& bag_filename,
|
||||
const std::map<std::string, size_t>& camera_topics);
|
||||
|
||||
virtual ~DataProviderRosbag() = default;
|
||||
|
||||
virtual bool spinOnce() override;
|
||||
|
||||
virtual bool ok() const override;
|
||||
|
||||
virtual size_t numCameras() const;
|
||||
|
||||
size_t size() const;
|
||||
|
||||
private:
|
||||
void loadRosbag(const std::string& bag_filename);
|
||||
void initBagView(const std::vector<std::string>& topics);
|
||||
|
||||
inline bool cameraSpin(const sensor_msgs::ImageConstPtr m_img,
|
||||
const rosbag::MessageInstance& m);
|
||||
|
||||
std::unique_ptr<rosbag::Bag> bag_;
|
||||
std::unique_ptr<rosbag::View> bag_view_;
|
||||
rosbag::View::iterator bag_view_it_;
|
||||
int n_processed_images_ = 0;
|
||||
|
||||
// subscribed topics:
|
||||
std::map<std::string, size_t> img_topic_camidx_map_; // camera_topic --> camera_id
|
||||
|
||||
SimulatorData sim_data_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <esim/rendering/renderer_base.hpp>
|
||||
#include <esim/rendering/simple_renderer_base.hpp>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat *img);
|
||||
Renderer::Ptr loadRendererFromGflags();
|
||||
SimpleRenderer::Ptr loadSimpleRendererFromGflags();
|
||||
|
||||
} // namespace event_camera_simulator
|
26
event_camera_simulator/esim_data_provider/package.xml
Normal file
26
event_camera_simulator/esim_data_provider/package.xml
Normal file
@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>esim_data_provider</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Data providers for the event camera simulator.</description>
|
||||
|
||||
<maintainer email="rebecq@ifi.uzh.ch">Henri Rebecq</maintainer>
|
||||
|
||||
<license>GPLv3</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>catkin_simple</buildtool_depend>
|
||||
|
||||
<depend>esim_common</depend>
|
||||
<depend>esim_rendering</depend>
|
||||
<depend>imp_planar_renderer</depend>
|
||||
<depend>imp_panorama_renderer</depend>
|
||||
<depend>imp_opengl_renderer</depend>
|
||||
<depend>imp_unrealcv_renderer</depend>
|
||||
<depend>imp_multi_objects_2d</depend>
|
||||
<depend>esim_trajectory</depend>
|
||||
<depend>ze_vi_simulation</depend>
|
||||
<depend>gflags_catkin</depend>
|
||||
<depend>glog_catkin</depend>
|
||||
|
||||
</package>
|
@ -0,0 +1,33 @@
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderBase::DataProviderBase(DataProviderType type)
|
||||
: type_(type)
|
||||
, signal_handler_(running_)
|
||||
{}
|
||||
|
||||
void DataProviderBase::spin()
|
||||
{
|
||||
while (ok())
|
||||
{
|
||||
spinOnce();
|
||||
}
|
||||
}
|
||||
|
||||
void DataProviderBase::pause()
|
||||
{
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
void DataProviderBase::shutdown()
|
||||
{
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
void DataProviderBase::registerCallback(const Callback& callback)
|
||||
{
|
||||
callback_ = callback;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,99 @@
|
||||
#include <ze/common/logging.hpp>
|
||||
#include <esim/data_provider/data_provider_factory.hpp>
|
||||
#include <esim/data_provider/data_provider_base.hpp>
|
||||
#include <esim/data_provider/data_provider_online_render.hpp>
|
||||
#include <esim/data_provider/data_provider_online_simple.hpp>
|
||||
#include <esim/data_provider/data_provider_from_folder.hpp>
|
||||
#include <esim/data_provider/data_provider_rosbag.hpp>
|
||||
|
||||
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_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_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(topic_cam0, "/cam0/image_raw", "");
|
||||
DEFINE_string(topic_cam1, "/cam1/image_raw", "");
|
||||
DEFINE_string(topic_cam2, "/cam2/image_raw", "");
|
||||
DEFINE_string(topic_cam3, "/cam3/image_raw", "");
|
||||
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";
|
||||
|
||||
// Fill camera topics.
|
||||
std::map<std::string, size_t> 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;
|
||||
}
|
||||
|
||||
} // namespace ze
|
@ -0,0 +1,88 @@
|
||||
#include <esim/data_provider/data_provider_from_folder.hpp>
|
||||
#include <ze/common/file_utils.hpp>
|
||||
#include <opencv2/imgcodecs/imgcodecs.hpp>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderFromFolder::DataProviderFromFolder(const std::string& path_to_data_folder)
|
||||
: DataProviderBase(DataProviderType::Folder),
|
||||
path_to_data_folder_(path_to_data_folder)
|
||||
{
|
||||
// Load CSV image file
|
||||
images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv"));
|
||||
CHECK(images_in_str_.is_open());
|
||||
|
||||
// Load camera rig
|
||||
camera_rig_ = ze::cameraRigFromYaml(ze::joinPath(path_to_data_folder, "calib.yaml"));
|
||||
CHECK(camera_rig_);
|
||||
CHECK_EQ(camera_rig_->size(), 1u) << "Only one camera in the rig is supported at the moment";
|
||||
|
||||
// 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))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if('%' != line.at(0) && '#' != line.at(0))
|
||||
{
|
||||
std::vector<std::string> 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_, "frame", "cam_0", items[1]);
|
||||
cv::Mat image = cv::imread(path_to_image, 0);
|
||||
CHECK(image.data) << "Could not load image from file: " << path_to_image;
|
||||
CHECK(image.rows == camera_rig_->at(0).height()
|
||||
&& image.cols == camera_rig_->at(0).width()) << "The image size in the data folder and the image size"
|
||||
"specified in the camera rig do not match";
|
||||
|
||||
VLOG(3) << "Read image from file: " << path_to_image;
|
||||
image.convertTo(*sim_data_.images[0], cv::DataType<ImageFloatType>::type, 1./255.);
|
||||
|
||||
if(callback_)
|
||||
{
|
||||
sim_data_.timestamp = static_cast<Time>(stamp);
|
||||
callback_(sim_data_);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DataProviderFromFolder::ok() const
|
||||
{
|
||||
if (!running_)
|
||||
{
|
||||
VLOG(1) << "Data Provider was paused/terminated.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,420 @@
|
||||
#include <esim/data_provider/data_provider_online_render.hpp>
|
||||
#include <esim/trajectory/trajectory_factory.hpp>
|
||||
#include <esim/trajectory/imu_factory.hpp>
|
||||
#include <esim/data_provider/renderer_factory.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
|
||||
#include <ze/cameras/camera_rig.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
#include <ze/common/timer_collection.hpp>
|
||||
|
||||
DECLARE_TIMER(TimerDataProvider, timers_data_provider_,
|
||||
render,
|
||||
optic_flow,
|
||||
sample_frame,
|
||||
sample_imu
|
||||
);
|
||||
|
||||
DEFINE_double(simulation_post_gaussian_blur_sigma, 0,
|
||||
"If sigma > 0, Gaussian blur the renderered images"
|
||||
"with a Gaussian filter standard deviation sigma.");
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderOnlineMoving3DCameraRig::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)
|
||||
: DataProviderBase(DataProviderType::RendererOnline),
|
||||
simulation_minimum_framerate_(simulation_minimum_framerate),
|
||||
simulation_imu_rate_(simulation_imu_rate),
|
||||
simulation_adaptive_sampling_method_(simulation_adaptive_sampling_method),
|
||||
simulation_adaptive_sampling_lambda_(simulation_adaptive_sampling_lambda),
|
||||
dt_imu_(1./simulation_imu_rate),
|
||||
dt_frame_(1./simulation_minimum_framerate)
|
||||
{
|
||||
CHECK(simulation_adaptive_sampling_method == 0
|
||||
|| simulation_adaptive_sampling_method == 1);
|
||||
|
||||
std::tie(trajectory_, trajectory_dyn_obj_) = loadTrajectorySimulatorFromGflags();
|
||||
imu_ = loadImuSimulatorFromGflags(trajectory_);
|
||||
camera_rig_ = ze::cameraRigFromGflags();
|
||||
|
||||
// Compute Field-of-Views for information
|
||||
const ze::Camera::Ptr camera = camera_rig_->atShared(0);
|
||||
const int width = camera->width();
|
||||
const int height = camera->height();
|
||||
|
||||
const ze::real_t horizontal_fov =
|
||||
180.0 / CV_PI *
|
||||
std::acos(camera->backProject(ze::Keypoint(0,height/2)).dot(
|
||||
camera->backProject(ze::Keypoint(width-1,height/2))));
|
||||
|
||||
const ze::real_t vertical_fov =
|
||||
180.0 / CV_PI *
|
||||
std::acos(camera->backProject(ze::Keypoint(width/2,0)).dot(
|
||||
camera->backProject(ze::Keypoint(width/2,height-1))));
|
||||
|
||||
const ze::real_t diagonal_fov =
|
||||
180.0 / CV_PI *
|
||||
std::acos(camera->backProject(ze::Keypoint(0,0)).dot(
|
||||
camera->backProject(ze::Keypoint(width-1,height-1))));
|
||||
|
||||
LOG(INFO) << "Horizontal FOV: " << horizontal_fov << " deg";
|
||||
LOG(INFO) << "Vertical FOV: " << vertical_fov << " deg";
|
||||
LOG(INFO) << "Diagonal FOV: " << diagonal_fov << " deg";
|
||||
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
renderers_.push_back(std::move(loadRendererFromGflags()));
|
||||
renderers_[i]->setCamera(camera_rig_->atShared(i));
|
||||
|
||||
optic_flow_helpers_.emplace_back(std::make_shared<OpticFlowHelper>(camera_rig_->atShared(i)));
|
||||
}
|
||||
|
||||
const size_t num_cameras = camera_rig_->size();
|
||||
|
||||
sim_data_.groundtruth.T_W_Cs.resize(num_cameras);
|
||||
sim_data_.groundtruth.angular_velocities_.resize(num_cameras);
|
||||
sim_data_.groundtruth.linear_velocities_.resize(num_cameras);
|
||||
for(size_t i=0; i<num_cameras; ++i)
|
||||
{
|
||||
const cv::Size size = cv::Size(camera_rig_->at(i).width(),
|
||||
camera_rig_->at(i).height());
|
||||
|
||||
sim_data_.images.emplace_back(ImagePtr(new Image(size)));
|
||||
sim_data_.depthmaps.emplace_back(DepthmapPtr(new Depthmap(size)));
|
||||
sim_data_.optic_flows.emplace_back(OpticFlowPtr(new OpticFlow(size)));
|
||||
|
||||
sim_data_.images[i]->setTo(0);
|
||||
sim_data_.depthmaps[i]->setTo(0);
|
||||
sim_data_.optic_flows[i]->setTo(0);
|
||||
}
|
||||
|
||||
for(size_t i=0; i<trajectory_dyn_obj_.size(); i++)
|
||||
{
|
||||
sim_data_.groundtruth.T_W_OBJ_.push_back(Transformation());
|
||||
sim_data_.groundtruth.linear_velocity_obj_.push_back(LinearVelocity());
|
||||
sim_data_.groundtruth.angular_velocity_obj_.push_back(AngularVelocity());
|
||||
}
|
||||
|
||||
sim_data_.camera_rig = camera_rig_;
|
||||
t_ = trajectory_->start();
|
||||
|
||||
// At the initial time, we sample everything (IMU + frames)
|
||||
sampleImu();
|
||||
sampleFrame();
|
||||
setAllUpdated();
|
||||
if(callback_)
|
||||
{
|
||||
callback_(sim_data_);
|
||||
}
|
||||
}
|
||||
|
||||
DataProviderOnlineMoving3DCameraRig::~DataProviderOnlineMoving3DCameraRig()
|
||||
{
|
||||
timers_data_provider_.saveToFile("/tmp", "data_provider_online_render.csv");
|
||||
}
|
||||
|
||||
size_t DataProviderOnlineMoving3DCameraRig::numCameras() const
|
||||
{
|
||||
if(camera_rig_)
|
||||
{
|
||||
return camera_rig_->size();
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::updateGroundtruth()
|
||||
{
|
||||
const Transformation T_W_B = trajectory_->T_W_B(t_);
|
||||
sim_data_.groundtruth.T_W_B = T_W_B;
|
||||
|
||||
const AngularVelocity omega_B = trajectory_->angularVelocity_B(t_);
|
||||
const LinearVelocity v_B_W = trajectory_->velocity_W(t_);
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
sim_data_.groundtruth.T_W_Cs[i] =
|
||||
sim_data_.groundtruth.T_W_B * camera_rig_->T_B_C(i);
|
||||
|
||||
const LinearVelocity v_W = v_B_W + T_W_B.getRotation().rotate(
|
||||
ze::skewSymmetric(omega_B) * camera_rig_->T_B_C(i).getPosition());
|
||||
|
||||
const AngularVelocity omega_C = camera_rig_->T_C_B(i).getRotation().rotate(omega_B);
|
||||
const LinearVelocity v_C = (camera_rig_->T_C_B(i) * T_W_B.inverse()).getRotation().rotate(v_W);
|
||||
|
||||
sim_data_.groundtruth.angular_velocities_[i] = omega_C;
|
||||
sim_data_.groundtruth.linear_velocities_[i] = v_C;
|
||||
}
|
||||
|
||||
// update poses of dynamic objects
|
||||
for (size_t i = 0; i < trajectory_dyn_obj_.size(); i++)
|
||||
{
|
||||
sim_data_.groundtruth.T_W_OBJ_[i] = trajectory_dyn_obj_[i]->T_W_B(t_);
|
||||
sim_data_.groundtruth.linear_velocity_obj_[i] = trajectory_dyn_obj_[i]->velocity_W(t_);
|
||||
sim_data_.groundtruth.angular_velocity_obj_[i] = sim_data_.groundtruth.T_W_OBJ_[i].getRotation().rotate(trajectory_dyn_obj_[i]->angularVelocity_B(t_));
|
||||
}
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::sampleImu()
|
||||
{
|
||||
// Sample new IMU (+ poses, twists, etc.) values,
|
||||
// Fill in the approriate data in sim_data
|
||||
auto t = timers_data_provider_[::TimerDataProvider::sample_imu].timeScope();
|
||||
|
||||
if(t_ > trajectory_->end())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
updateGroundtruth();
|
||||
sim_data_.specific_force_corrupted = imu_->specificForceCorrupted(t_);
|
||||
sim_data_.angular_velocity_corrupted = imu_->angularVelocityCorrupted(t_);
|
||||
sim_data_.groundtruth.acc_bias = imu_->bias()->accelerometer(t_);
|
||||
sim_data_.groundtruth.gyr_bias = imu_->bias()->gyroscope(t_);
|
||||
|
||||
last_t_imu_ = t_;
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::sampleFrame()
|
||||
{
|
||||
// Sample (i.e. render) a new frame (+ depth map),
|
||||
// Fill in the appropriate data in sim_data
|
||||
// Compute the optic flow and compute the next latest sampling time in order
|
||||
// to guarantee that the displacement is bounded by simulation_max_displacement_successive_frames
|
||||
auto t = timers_data_provider_[::TimerDataProvider::sample_frame].timeScope();
|
||||
|
||||
if(t_ > trajectory_->end())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
updateGroundtruth();
|
||||
|
||||
{
|
||||
auto t = timers_data_provider_[::TimerDataProvider::render].timeScope();
|
||||
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
CHECK(renderers_[i]);
|
||||
|
||||
// if the renderer provides a function to compute the optic
|
||||
// flow (for example, the OpenGL renderer which implements
|
||||
// optic flow computation efficiently using a shader), use that.
|
||||
// otherwise, compute the optic flow ourselves using the renderer depthmap
|
||||
if(renderers_[i]->canComputeOpticFlow())
|
||||
{
|
||||
renderers_[i]->renderWithFlow(sim_data_.groundtruth.T_W_B * camera_rig_->T_B_C(i),
|
||||
sim_data_.groundtruth.linear_velocities_[i],
|
||||
sim_data_.groundtruth.angular_velocities_[i],
|
||||
sim_data_.groundtruth.T_W_OBJ_,
|
||||
sim_data_.groundtruth.linear_velocity_obj_,
|
||||
sim_data_.groundtruth.angular_velocity_obj_,
|
||||
sim_data_.images[i],
|
||||
sim_data_.depthmaps[i],
|
||||
sim_data_.optic_flows[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
renderers_[i]->render(sim_data_.groundtruth.T_W_B * camera_rig_->T_B_C(i),
|
||||
sim_data_.groundtruth.T_W_OBJ_,
|
||||
sim_data_.images[i],
|
||||
sim_data_.depthmaps[i]);
|
||||
}
|
||||
|
||||
// Optionally blur the rendered images slightly
|
||||
if(FLAGS_simulation_post_gaussian_blur_sigma > 0)
|
||||
{
|
||||
gaussianBlur(sim_data_.images[i], FLAGS_simulation_post_gaussian_blur_sigma);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
auto t = timers_data_provider_[::TimerDataProvider::optic_flow].timeScope();
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
CHECK(optic_flow_helpers_[i]);
|
||||
if(!renderers_[i]->canComputeOpticFlow())
|
||||
{
|
||||
optic_flow_helpers_[i]->computeFromDepthAndTwist(sim_data_.groundtruth.angular_velocities_[i],
|
||||
sim_data_.groundtruth.linear_velocities_[i],
|
||||
sim_data_.depthmaps[i], sim_data_.optic_flows[i]);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Adaptive sampling scheme based on predicted brightness change
|
||||
if(simulation_adaptive_sampling_method_ == 0)
|
||||
{
|
||||
// Predict brightness change based on image gradient and optic flow
|
||||
std::vector<FloatType> max_dLdts;
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
max_dLdts.push_back(
|
||||
maxPredictedAbsBrightnessChange(sim_data_.images[i],
|
||||
sim_data_.optic_flows[i]));
|
||||
}
|
||||
const FloatType max_dLdt = *std::max_element(max_dLdts.begin(),
|
||||
max_dLdts.end());
|
||||
|
||||
VLOG(1) << "max(|dLdt|) = " << max_dLdt << " logDN/s";
|
||||
|
||||
// Compute next sampling time
|
||||
// t_{k+1} = t_k + delta_t where
|
||||
// delta_t = lambda / max(|dL/dt|)
|
||||
const ze::real_t delta_t = simulation_adaptive_sampling_lambda_ / max_dLdt;
|
||||
VLOG(1) << "deltaT = " << 1000.0 * delta_t << " ms";
|
||||
|
||||
next_t_flow_ = t_ + delta_t;
|
||||
}
|
||||
|
||||
// Adaptive sampling scheme based on optic flow
|
||||
else {
|
||||
std::vector<FloatType> max_flow_magnitudes;
|
||||
for(size_t i=0; i<camera_rig_->size(); ++i)
|
||||
{
|
||||
max_flow_magnitudes.push_back(maxMagnitudeOpticFlow(sim_data_.optic_flows[i]));
|
||||
}
|
||||
const FloatType max_flow_magnitude = *std::max_element(max_flow_magnitudes.begin(), max_flow_magnitudes.end());
|
||||
|
||||
VLOG(1) << "max(||optic_flow||) = " << max_flow_magnitude << " px/s";
|
||||
|
||||
// Compute next sampling time
|
||||
// t_{k+1} = t_k + delta_t where
|
||||
// delta_t = lambda / max(||optic_flow||)
|
||||
const ze::real_t delta_t = simulation_adaptive_sampling_lambda_ / max_flow_magnitude;
|
||||
VLOG(1) << "deltaT = " << 1000.0 * delta_t << " ms";
|
||||
|
||||
next_t_flow_ = t_ + delta_t;
|
||||
}
|
||||
|
||||
last_t_frame_ = t_;
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::setImuUpdated()
|
||||
{
|
||||
// Set all the IMU-related flags to true, and all the frame-related flags to false
|
||||
sim_data_.imu_updated = true;
|
||||
sim_data_.twists_updated = true;
|
||||
sim_data_.poses_updated = true;
|
||||
sim_data_.images_updated = false;
|
||||
sim_data_.depthmaps_updated = false;
|
||||
sim_data_.optic_flows_updated = false;
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::setFrameUpdated()
|
||||
{
|
||||
// Set all the frame-related flags to true, and all the IMU-related flags to false
|
||||
sim_data_.imu_updated = false;
|
||||
sim_data_.twists_updated = true;
|
||||
sim_data_.poses_updated = true;
|
||||
sim_data_.images_updated = true;
|
||||
sim_data_.depthmaps_updated = true;
|
||||
sim_data_.optic_flows_updated = true;
|
||||
}
|
||||
|
||||
void DataProviderOnlineMoving3DCameraRig::setAllUpdated()
|
||||
{
|
||||
// Set all the flags to true to indicated everything has been changed
|
||||
sim_data_.imu_updated = true;
|
||||
sim_data_.twists_updated = true;
|
||||
sim_data_.poses_updated = true;
|
||||
sim_data_.images_updated = true;
|
||||
sim_data_.depthmaps_updated = true;
|
||||
sim_data_.optic_flows_updated = true;
|
||||
}
|
||||
|
||||
bool DataProviderOnlineMoving3DCameraRig::spinOnce()
|
||||
{
|
||||
/* At what time do we need to sample "something" (and what "something"?)
|
||||
We choose the next sampling time by considering the following constraints:
|
||||
|
||||
1. The IMU sampling rate must be constant (typically, from 200 Hz to 1 kHz)
|
||||
2. The frame sampling rate must be greater than a minimum value (typically, from 20 Hz to 100 Hz)
|
||||
3. The pixel displacement between two successive frames must be lower than a threshold.
|
||||
|
||||
* If the next sample needs to be an IMU sample, we just sample a new IMU value, without regenerating a frame,
|
||||
and transmit only the new IMU (+ poses, twists, IMU bias) to the publisher by setting the approriate "data_changed" flags in the
|
||||
sim_data structure.
|
||||
|
||||
* If the next sample needs to be a frame sample, we render a new frame (+ depth map + optic flow), but not a new IMU value.
|
||||
This can happen either because
|
||||
(i) a new frame must be rendered in order to guarantee that the displacement between frames is bounded, or
|
||||
(ii) the frame rate should be higher than a minimum (used-defined) value.
|
||||
|
||||
At the beginning of time (t_ = trajectory_->start()), we sample everything (IMU + frame).
|
||||
*/
|
||||
|
||||
const ze::real_t next_t_imu = last_t_imu_ + dt_imu_;
|
||||
const ze::real_t next_t_frame = last_t_frame_ + dt_frame_;
|
||||
|
||||
VLOG(2) << "t = " << t_;
|
||||
VLOG(2) << "next_t_imu = " << next_t_imu;
|
||||
VLOG(2) << "next_t_frame = " << next_t_frame;
|
||||
VLOG(2) << "next_t_flow = " << next_t_flow_;
|
||||
|
||||
if(next_t_imu < next_t_frame && next_t_imu < next_t_flow_)
|
||||
{
|
||||
VLOG(2) << "Sample IMU";
|
||||
t_ = next_t_imu;
|
||||
sampleImu();
|
||||
setImuUpdated();
|
||||
}
|
||||
else if(next_t_flow_ < next_t_imu && next_t_flow_ < next_t_imu)
|
||||
{
|
||||
VLOG(2) << "Sample frame (because of optic flow)";
|
||||
t_ = next_t_flow_;
|
||||
sampleFrame();
|
||||
setFrameUpdated();
|
||||
}
|
||||
else if(next_t_frame < next_t_imu && next_t_frame < next_t_flow_)
|
||||
{
|
||||
VLOG(2) << "Sample frame (because of minimum framerate)";
|
||||
t_ = next_t_frame;
|
||||
sampleFrame();
|
||||
setFrameUpdated();
|
||||
}
|
||||
else
|
||||
{
|
||||
VLOG(2) << "Sample IMU and frame";
|
||||
t_ = next_t_frame;
|
||||
// In that case, we sample everything
|
||||
sampleImu();
|
||||
sampleFrame();
|
||||
setAllUpdated();
|
||||
}
|
||||
|
||||
if(t_ > trajectory_->end())
|
||||
{
|
||||
running_ = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
if(callback_)
|
||||
{
|
||||
sim_data_.timestamp = static_cast<Time>(ze::secToNanosec(t_));
|
||||
callback_(sim_data_);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_FIRST_N(WARNING, 1) << "No camera callback registered but measurements available.";
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DataProviderOnlineMoving3DCameraRig::ok() const
|
||||
{
|
||||
if (!running_)
|
||||
{
|
||||
VLOG(1) << "Data Provider was paused/terminated.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,177 @@
|
||||
#include <esim/data_provider/data_provider_online_simple.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
#include <esim/data_provider/renderer_factory.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
|
||||
DECLARE_double(simulation_post_gaussian_blur_sigma);
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderOnlineSimple::DataProviderOnlineSimple(ze::real_t simulation_minimum_framerate,
|
||||
int simulation_adaptive_sampling_method,
|
||||
ze::real_t simulation_adaptive_sampling_lambda)
|
||||
: DataProviderBase(DataProviderType::RendererOnline),
|
||||
simulation_minimum_framerate_(simulation_minimum_framerate),
|
||||
simulation_adaptive_sampling_method_(simulation_adaptive_sampling_method),
|
||||
simulation_adaptive_sampling_lambda_(simulation_adaptive_sampling_lambda),
|
||||
dt_frame_(1./simulation_minimum_framerate)
|
||||
{
|
||||
CHECK(simulation_adaptive_sampling_method == 0
|
||||
|| simulation_adaptive_sampling_method == 1);
|
||||
|
||||
renderer_ = loadSimpleRendererFromGflags();
|
||||
|
||||
const size_t num_cameras = 1u;
|
||||
|
||||
for(size_t i=0; i<num_cameras; ++i)
|
||||
{
|
||||
const cv::Size size = cv::Size(renderer_->getWidth(),
|
||||
renderer_->getHeight());
|
||||
|
||||
sim_data_.images.emplace_back(ImagePtr(new Image(size)));
|
||||
sim_data_.optic_flows.emplace_back(OpticFlowPtr(new OpticFlow(size)));
|
||||
|
||||
sim_data_.images[i]->setTo(0);
|
||||
sim_data_.optic_flows[i]->setTo(0);
|
||||
}
|
||||
|
||||
// At the initial time, we sample a frame + optic flow map
|
||||
t_ = 0.;
|
||||
sampleFrame();
|
||||
setFrameUpdated();
|
||||
if(callback_)
|
||||
{
|
||||
callback_(sim_data_);
|
||||
}
|
||||
}
|
||||
|
||||
DataProviderOnlineSimple::~DataProviderOnlineSimple()
|
||||
{
|
||||
}
|
||||
|
||||
size_t DataProviderOnlineSimple::numCameras() const
|
||||
{
|
||||
return 1u;
|
||||
}
|
||||
|
||||
bool DataProviderOnlineSimple::sampleFrame()
|
||||
{
|
||||
// Sample (i.e. render) a new frame (+ optic flow map),
|
||||
// Fill in the appropriate data in sim_data
|
||||
// Compute the optic flow and compute the next latest sampling time in order
|
||||
// to guarantee that the displacement is bounded by simulation_max_displacement_successive_frames
|
||||
CHECK(renderer_);
|
||||
bool is_finished = renderer_->render(ze::secToNanosec(t_),
|
||||
sim_data_.images[0],
|
||||
sim_data_.optic_flows[0]);
|
||||
|
||||
if(is_finished)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// Optionally blur the rendered images slightly
|
||||
if(FLAGS_simulation_post_gaussian_blur_sigma > 0)
|
||||
{
|
||||
gaussianBlur(sim_data_.images[0], FLAGS_simulation_post_gaussian_blur_sigma);
|
||||
}
|
||||
|
||||
// Adaptive sampling scheme based on predicted brightness change
|
||||
if(simulation_adaptive_sampling_method_ == 0)
|
||||
{
|
||||
// Predict brightness change based on image gradient and optic flow
|
||||
const FloatType max_dLdt = maxPredictedAbsBrightnessChange(sim_data_.images[0],
|
||||
sim_data_.optic_flows[0]);
|
||||
|
||||
VLOG(1) << "max(|dLdt|) = " << max_dLdt << " logDN/s";
|
||||
|
||||
// Compute next sampling time
|
||||
// t_{k+1} = t_k + delta_t where
|
||||
// delta_t = lambda / max(|dL/dt|)
|
||||
const ze::real_t delta_t = simulation_adaptive_sampling_lambda_ / max_dLdt;
|
||||
VLOG(1) << "deltaT = " << 1000.0 * delta_t << " ms";
|
||||
|
||||
next_t_adaptive_ = t_ + delta_t;
|
||||
}
|
||||
|
||||
// Adaptive sampling scheme based on optic flow
|
||||
else {
|
||||
const FloatType max_flow_magnitude = maxMagnitudeOpticFlow(sim_data_.optic_flows[0]);
|
||||
|
||||
VLOG(1) << "max(||optic_flow||) = " << max_flow_magnitude << " px/s";
|
||||
|
||||
// Compute next sampling time
|
||||
// t_{k+1} = t_k + delta_t where
|
||||
// delta_t = lambda / max(||optic_flow||)
|
||||
const ze::real_t delta_t = simulation_adaptive_sampling_lambda_ / max_flow_magnitude;
|
||||
VLOG(1) << "deltaT = " << 1000.0 * delta_t << " ms";
|
||||
|
||||
next_t_adaptive_ = t_ + delta_t;
|
||||
}
|
||||
|
||||
last_t_frame_ = t_;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void DataProviderOnlineSimple::setFrameUpdated()
|
||||
{
|
||||
// Set all the frame-related flags to true, and all the IMU-related flags to false
|
||||
sim_data_.imu_updated = false;
|
||||
sim_data_.twists_updated = false;
|
||||
sim_data_.poses_updated = false;
|
||||
sim_data_.images_updated = true;
|
||||
sim_data_.depthmaps_updated = false;
|
||||
sim_data_.optic_flows_updated = true;
|
||||
}
|
||||
|
||||
bool DataProviderOnlineSimple::spinOnce()
|
||||
{
|
||||
const ze::real_t next_t_frame = last_t_frame_ + dt_frame_;
|
||||
|
||||
VLOG(2) << "t = " << t_;
|
||||
VLOG(2) << "next_t_frame = " << next_t_frame;
|
||||
VLOG(2) << "next_t_flow = " << next_t_adaptive_;
|
||||
|
||||
if(next_t_adaptive_ < next_t_frame)
|
||||
{
|
||||
VLOG(2) << "Sample frame (because of optic flow)";
|
||||
t_ = next_t_adaptive_;
|
||||
}
|
||||
else
|
||||
{
|
||||
VLOG(2) << "Sample frame (because of minimum framerate)";
|
||||
t_ = next_t_frame;
|
||||
}
|
||||
bool is_finished = sampleFrame();
|
||||
setFrameUpdated();
|
||||
|
||||
if(is_finished)
|
||||
{
|
||||
running_ = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
if(callback_)
|
||||
{
|
||||
sim_data_.timestamp = static_cast<Time>(ze::secToNanosec(t_));
|
||||
callback_(sim_data_);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_FIRST_N(WARNING, 1) << "No camera callback registered but measurements available.";
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DataProviderOnlineSimple::ok() const
|
||||
{
|
||||
if (!running_)
|
||||
{
|
||||
VLOG(1) << "Data Provider was paused/terminated.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,242 @@
|
||||
#include <esim/data_provider/data_provider_rosbag.hpp>
|
||||
|
||||
#include <ze/common/logging.hpp>
|
||||
#include <rosbag/query.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
#include <ze/common/string_utils.hpp>
|
||||
#include <ze/common/path_utils.hpp>
|
||||
|
||||
DEFINE_int32(data_source_stop_after_n_frames, -1,
|
||||
"How many frames should be processed?");
|
||||
DEFINE_double(data_source_start_time_s, 0.0,
|
||||
"Start time in seconds");
|
||||
DEFINE_double(data_source_stop_time_s, 0.0,
|
||||
"Stop time in seconds");
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
DataProviderRosbag::DataProviderRosbag(
|
||||
const std::string& bag_filename,
|
||||
const std::map<std::string, size_t>& img_topic_camidx_map)
|
||||
: DataProviderBase(DataProviderType::Rosbag)
|
||||
, img_topic_camidx_map_(img_topic_camidx_map)
|
||||
{
|
||||
loadRosbag(bag_filename);
|
||||
|
||||
std::vector<std::string> topics;
|
||||
for (auto it : img_topic_camidx_map_)
|
||||
{
|
||||
VLOG(1) << "Subscribing to: " << it.first;
|
||||
topics.push_back(it.first);
|
||||
sim_data_.images.emplace_back(ImagePtr(new Image()));
|
||||
}
|
||||
|
||||
initBagView(topics);
|
||||
}
|
||||
|
||||
void DataProviderRosbag::loadRosbag(const std::string& bag_filename)
|
||||
{
|
||||
CHECK(ze::fileExists(bag_filename)) << "File does not exist: " << bag_filename;
|
||||
VLOG(1) << "Opening rosbag: " << bag_filename << " ...";
|
||||
bag_.reset(new rosbag::Bag);
|
||||
try
|
||||
{
|
||||
bag_->open(bag_filename, rosbag::bagmode::Read);
|
||||
}
|
||||
catch (const std::exception e)
|
||||
{
|
||||
LOG(FATAL) << "Could not open rosbag " << bag_filename << ": " << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
void DataProviderRosbag::initBagView(const std::vector<std::string>& topics)
|
||||
{
|
||||
bag_view_.reset(new rosbag::View(*bag_, rosbag::TopicQuery(topics)));
|
||||
if (FLAGS_data_source_start_time_s != 0.0 ||
|
||||
FLAGS_data_source_stop_time_s != 0.0)
|
||||
{
|
||||
CHECK_GE(FLAGS_data_source_start_time_s, 0);
|
||||
CHECK_GE(FLAGS_data_source_stop_time_s, 0);
|
||||
|
||||
// Retrieve begin and end times from the bag file (given the topic query).
|
||||
const ros::Time absolute_time_offset = bag_view_->getBeginTime();
|
||||
VLOG(2) << "Bag begin time: " << absolute_time_offset;
|
||||
const ros::Time absolute_end_time = bag_view_->getEndTime();
|
||||
VLOG(2) << "Bag end time: " << absolute_end_time;
|
||||
if (absolute_end_time < absolute_time_offset)
|
||||
{
|
||||
LOG(FATAL) << "Invalid bag end time: "
|
||||
<< absolute_end_time
|
||||
<< ". Check that the bag file is properly indexed"
|
||||
<< " by running 'rosbag reindex file.bag'.";
|
||||
}
|
||||
|
||||
// Compute start and stop time.
|
||||
LOG(INFO) << "Starting rosbag at time: " << FLAGS_data_source_start_time_s;
|
||||
const ros::Duration data_source_start_time(FLAGS_data_source_start_time_s);
|
||||
const ros::Time absolute_start_time =
|
||||
data_source_start_time.isZero() ?
|
||||
absolute_time_offset : absolute_time_offset + data_source_start_time;
|
||||
const ros::Duration data_source_stop_time(FLAGS_data_source_stop_time_s);
|
||||
const ros::Time absolute_stop_time =
|
||||
data_source_stop_time.isZero() ?
|
||||
absolute_end_time : absolute_time_offset + data_source_stop_time;
|
||||
|
||||
// Ensure that the provided stop time is valid.
|
||||
// When a bag file is corrupted / invalid the bag end time
|
||||
// cannot be retrieved. Run rosbag info to check if the bag file
|
||||
// is properly indexed.
|
||||
if (absolute_stop_time < absolute_start_time)
|
||||
{
|
||||
LOG(ERROR) << "Provided stop time is less than bag begin time. "
|
||||
<< "Please make sure to provide a valid stop time and "
|
||||
<< "check that the bag file is properly indexed "
|
||||
<< "by running 'rosbag reindex file.bag'.";
|
||||
}
|
||||
else if (absolute_stop_time > absolute_end_time)
|
||||
{
|
||||
LOG(ERROR) << "Provided stop time is greater than bag end time. "
|
||||
<< "Please make sure to provide a valid stop time and "
|
||||
<< "check that the bag file is properly indexed "
|
||||
<< "by running 'rosbag reindex file.bag'.";
|
||||
}
|
||||
else
|
||||
{
|
||||
VLOG(1) << "Absolute start time set to " << absolute_start_time;
|
||||
VLOG(1) << "Absolute stop time set to " << absolute_stop_time;
|
||||
}
|
||||
|
||||
// Reset the bag View
|
||||
CHECK_GT(absolute_stop_time, absolute_start_time);
|
||||
CHECK_LE(absolute_stop_time, absolute_end_time);
|
||||
bag_view_.reset(new rosbag::View(*bag_, rosbag::TopicQuery(topics),
|
||||
absolute_start_time, absolute_stop_time));
|
||||
}
|
||||
bag_view_it_ = bag_view_->begin();
|
||||
|
||||
// Ensure that topics exist
|
||||
// The connection info only contains topics that are available in the bag
|
||||
// If a topic is requested that is not avaiable, it does not show up in the info.
|
||||
std::vector<const rosbag::ConnectionInfo*> connection_infos =
|
||||
bag_view_->getConnections();
|
||||
if (topics.size() != connection_infos.size())
|
||||
{
|
||||
LOG(ERROR) << "Successfully connected to " << connection_infos.size() << " topics:";
|
||||
for (const rosbag::ConnectionInfo* info : connection_infos)
|
||||
{
|
||||
LOG(ERROR) << "*) " << info->topic;
|
||||
}
|
||||
LOG(ERROR) << "Requested " << topics.size() << " topics:";
|
||||
for (const std::string topic : topics)
|
||||
{
|
||||
LOG(ERROR) << "*) " << topic;
|
||||
}
|
||||
LOG(FATAL) << "Not all requested topics founds in bagfile. "
|
||||
<< "Is topic_cam0, topic_imu0, etc. set correctly? "
|
||||
<< "Maybe removing/adding a slash as prefix solves the problem.";
|
||||
}
|
||||
}
|
||||
|
||||
size_t DataProviderRosbag::numCameras() const
|
||||
{
|
||||
return img_topic_camidx_map_.size();
|
||||
}
|
||||
|
||||
bool DataProviderRosbag::spinOnce()
|
||||
{
|
||||
if (bag_view_it_ != bag_view_->end())
|
||||
{
|
||||
const rosbag::MessageInstance m = *bag_view_it_;
|
||||
|
||||
// Camera Messages:
|
||||
const sensor_msgs::ImageConstPtr m_img = m.instantiate<sensor_msgs::Image>();
|
||||
if (m_img && callback_)
|
||||
{
|
||||
if (!cameraSpin(m_img, m))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_FIRST_N(WARNING, 1) << "No camera callback registered but measurements available";
|
||||
}
|
||||
|
||||
++bag_view_it_;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool DataProviderRosbag::cameraSpin(sensor_msgs::ImageConstPtr m_img,
|
||||
const rosbag::MessageInstance& m)
|
||||
{
|
||||
auto it = img_topic_camidx_map_.find(m.getTopic());
|
||||
if (it != img_topic_camidx_map_.end())
|
||||
{
|
||||
++n_processed_images_;
|
||||
if (FLAGS_data_source_stop_after_n_frames > 0 &&
|
||||
n_processed_images_ > FLAGS_data_source_stop_after_n_frames)
|
||||
{
|
||||
LOG(WARNING) << "Data source has reached max number of desired frames.";
|
||||
running_ = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
cv_bridge::CvImagePtr cv_ptr;
|
||||
|
||||
try
|
||||
{
|
||||
cv_ptr = cv_bridge::toCvCopy(m_img, sensor_msgs::image_encodings::MONO8);
|
||||
}
|
||||
catch (cv_bridge::Exception& e)
|
||||
{
|
||||
LOG(WARNING) << "cv_bridge exception: %s", e.what();
|
||||
return false;
|
||||
}
|
||||
|
||||
cv_ptr->image.convertTo(*(sim_data_.images[0]), cv::DataType<ImageFloatType>::type, 1./255.);
|
||||
sim_data_.timestamp = static_cast<Time>(m_img->header.stamp.toNSec());
|
||||
|
||||
sim_data_.imu_updated = false;
|
||||
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;
|
||||
|
||||
callback_(sim_data_);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DataProviderRosbag::ok() const
|
||||
{
|
||||
if (!running_)
|
||||
{
|
||||
VLOG(1) << "Data Provider was paused/terminated.";
|
||||
return false;
|
||||
}
|
||||
if (bag_view_it_ == bag_view_->end())
|
||||
{
|
||||
VLOG(1) << "All data processed.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t DataProviderRosbag::size() const
|
||||
{
|
||||
CHECK(bag_view_);
|
||||
return bag_view_->size();
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
@ -0,0 +1,199 @@
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <esim/data_provider/renderer_factory.hpp>
|
||||
#include <esim/imp_planar_renderer/planar_renderer.hpp>
|
||||
#include <esim/imp_panorama_renderer/panorama_renderer.hpp>
|
||||
#include <esim/imp_opengl_renderer/opengl_renderer.hpp>
|
||||
#include <esim/imp_unrealcv_renderer/unrealcv_renderer.hpp>
|
||||
#include <esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp>
|
||||
#include <ze/cameras/camera_models.hpp>
|
||||
#include <ze/cameras/camera_impl.hpp>
|
||||
#include <ze/common/logging.hpp>
|
||||
#include <opencv2/imgcodecs/imgcodecs.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
DEFINE_int32(renderer_type, 0, " 0: Planar renderer, 1: Panorama renderer, 2: OpenGL renderer");
|
||||
|
||||
DEFINE_string(renderer_texture, "",
|
||||
"Path to image which will be used to texture the plane");
|
||||
|
||||
DEFINE_double(renderer_hfov_cam_source_deg, 130.0,
|
||||
"Horizontal FoV of the source camera (that captured the image on the plane)");
|
||||
|
||||
DEFINE_double(renderer_preprocess_median_blur, 0,
|
||||
"Kernel size of the preprocessing median blur.");
|
||||
|
||||
DEFINE_double(renderer_preprocess_gaussian_blur, 0,
|
||||
"Amount of preprocessing Gaussian blur.");
|
||||
|
||||
DEFINE_double(renderer_plane_x, 0.0,
|
||||
"x position of the center of the plane, in world coordinates");
|
||||
|
||||
DEFINE_double(renderer_plane_y, 0.0,
|
||||
"y position of the center of the plane, in world coordinates");
|
||||
|
||||
DEFINE_double(renderer_plane_z, -1.0,
|
||||
"z position of the center of the plane, in world coordinates");
|
||||
|
||||
DEFINE_double(renderer_plane_qw, 0.0,
|
||||
"w component of the quaternion q_W_P (orientation of the plane with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_plane_qx, 1.0,
|
||||
"x component of the quaternion q_W_P (orientation of the plane with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_plane_qy, 0.0,
|
||||
"y component of the quaternion q_W_P (orientation of the plane with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_plane_qz, 0.0,
|
||||
"z component of the quaternion q_W_P (orientation of the plane with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_z_min, 0.01,
|
||||
"Minimum clipping distance.");
|
||||
|
||||
DEFINE_double(renderer_z_max, 10.0,
|
||||
"Maximum clipping distance.");
|
||||
|
||||
DEFINE_bool(renderer_extend_border, false,
|
||||
"Whether to extend the borders of the plane to infinity or not.");
|
||||
|
||||
DEFINE_double(renderer_panorama_qw, 0.0,
|
||||
"w component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_panorama_qx, 1.0,
|
||||
"x component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_panorama_qy, 0.0,
|
||||
"y component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
|
||||
|
||||
DEFINE_double(renderer_panorama_qz, 0.0,
|
||||
"z component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img)
|
||||
{
|
||||
CHECK(img);
|
||||
VLOG(1) << "Loading texture file from file: " << FLAGS_renderer_texture << ".";
|
||||
*img = cv::imread(path_to_img, 0);
|
||||
if(!img->data)
|
||||
{
|
||||
LOG(FATAL) << "Could not open image at: " << FLAGS_renderer_texture << ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
if(FLAGS_renderer_preprocess_median_blur > 1)
|
||||
{
|
||||
VLOG(1) << "Pre-filtering the texture with median filter of size: "
|
||||
<< FLAGS_renderer_preprocess_median_blur << ".";
|
||||
cv::medianBlur(*img, *img, FLAGS_renderer_preprocess_median_blur);
|
||||
}
|
||||
|
||||
if(FLAGS_renderer_preprocess_gaussian_blur > 0)
|
||||
{
|
||||
VLOG(1) << "Pre-filtering the texture with gaussian filter of size: "
|
||||
<< FLAGS_renderer_preprocess_gaussian_blur << ".";
|
||||
cv::GaussianBlur(*img, *img, cv::Size(21,21), FLAGS_renderer_preprocess_gaussian_blur);
|
||||
}
|
||||
|
||||
img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0/255.0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Renderer::Ptr loadRendererFromGflags()
|
||||
{
|
||||
Renderer::Ptr renderer;
|
||||
|
||||
switch (FLAGS_renderer_type)
|
||||
{
|
||||
case 0: // Planar renderer
|
||||
{
|
||||
cv::Mat img_src;
|
||||
if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
double f_src = hfovToFocalLength(FLAGS_renderer_hfov_cam_source_deg, img_src.cols);
|
||||
Camera::Ptr cam_src = std::make_shared<ze::PinholeCamera>(
|
||||
img_src.cols, img_src.rows, ze::CameraType::Pinhole,
|
||||
(Vector4() << f_src, f_src, 0.5 * img_src.cols, 0.5 * img_src.rows).finished(),
|
||||
ze::VectorX());
|
||||
|
||||
Transformation T_W_P;
|
||||
T_W_P.getPosition() = ze::Position(FLAGS_renderer_plane_x,
|
||||
FLAGS_renderer_plane_y,
|
||||
FLAGS_renderer_plane_z);
|
||||
|
||||
T_W_P.getRotation() = ze::Quaternion(FLAGS_renderer_plane_qw,
|
||||
FLAGS_renderer_plane_qx,
|
||||
FLAGS_renderer_plane_qy,
|
||||
FLAGS_renderer_plane_qz);
|
||||
|
||||
renderer.reset(new PlanarRenderer(img_src, cam_src,
|
||||
T_W_P,
|
||||
FLAGS_renderer_z_min,
|
||||
FLAGS_renderer_z_max,
|
||||
FLAGS_renderer_extend_border));
|
||||
|
||||
break;
|
||||
}
|
||||
case 1: // Panorama renderer
|
||||
{
|
||||
cv::Mat img_src;
|
||||
if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
Transformation::Rotation R_W_P;
|
||||
R_W_P = ze::Quaternion(FLAGS_renderer_panorama_qw,
|
||||
FLAGS_renderer_panorama_qx,
|
||||
FLAGS_renderer_panorama_qy,
|
||||
FLAGS_renderer_panorama_qz);
|
||||
|
||||
renderer.reset(new PanoramaRenderer(img_src, R_W_P));
|
||||
break;
|
||||
}
|
||||
case 2: // OpenGL renderer
|
||||
{
|
||||
renderer.reset(new OpenGLRenderer());
|
||||
break;
|
||||
}
|
||||
case 3: // UnrealCV renderer
|
||||
{
|
||||
renderer.reset(new UnrealCvRenderer());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
LOG(FATAL) << "Renderer type is not known.";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return renderer;
|
||||
}
|
||||
|
||||
SimpleRenderer::Ptr loadSimpleRendererFromGflags()
|
||||
{
|
||||
SimpleRenderer::Ptr renderer;
|
||||
|
||||
switch (FLAGS_renderer_type)
|
||||
{
|
||||
case 0: // Multi-objects 2D renderer
|
||||
{
|
||||
renderer.reset(new MultiObject2DRenderer());
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
LOG(FATAL) << "Renderer type is not known.";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return renderer;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
Reference in New Issue
Block a user