initial commit

This commit is contained in:
Henri Rebecq
2018-10-29 17:53:15 +01:00
commit a8c2f0ca43
208 changed files with 554184 additions and 0 deletions

View 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()

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View 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>

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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