karma/hdr_esim
1
1
Fork 0
mirror of https://github.com/karma-riuk/hdr_esim.git synced 2024-11-23 14:47:50 +01:00
Code Issues Packages Projects Releases Wiki Activity

Reformated the project to make it more readable (to me)

Browse Source
This commit is contained in:
Arnaud Fauconnet 2023-07-17 11:45:28 +02:00
parent de6743207d
commit 4738ae7444
66 changed files with 6713 additions and 5880 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp
View File

@ -1,18 +1,16 @@
#pragma once
#include <esim/common/types.hpp>
#include <deque>
#include <esim/common/types.hpp>
#include <ze/common/time_conversions.hpp>
namespace event_camera_simulator {
class ImageBuffer
{
public:
class ImageBuffer {
public:
ZE_POINTER_TYPEDEFS(ImageBuffer);
struct ImageData
{
struct ImageData {
ImageData(Image img, Time stamp, Duration exposure_time)
: image(img),
stamp(stamp),
@ -20,30 +18,35 @@ public:
Image image;
Time stamp;
Duration exposure_time; // timestamp since last image (0 if this is the first image)
Duration exposure_time; // timestamp since last image (0 if this is
// the first image)
};
using ExposureImage = std::pair<Duration, Image>;
// Rolling image buffer of mazimum size 'buffer_size_ns'.
ImageBuffer(Duration buffer_size_ns)
: buffer_size_ns_(buffer_size_ns) {}
ImageBuffer(Duration buffer_size_ns): buffer_size_ns_(buffer_size_ns) {}
void addImage(Time t, const Image& img);
std::deque<ImageData> getRawBuffer() const { return data_; }
std::deque<ImageData> getRawBuffer() const {
return data_;
}
size_t size() const { return data_.size(); }
size_t size() const {
return data_.size();
}
Duration getExposureTime() const { return buffer_size_ns_; }
Duration getExposureTime() const {
return buffer_size_ns_;
}
private:
private:
Duration buffer_size_ns_;
std::deque<ImageData> data_;
};
};
/*
/*
* The CameraSimulator takes as input a sequence of stamped images,
* assumed to be sampled at a "sufficiently high" framerate and with
* floating-point precision, and treats each image as a measure of
@ -52,21 +55,20 @@ private:
*
* @TODO: simulate a non-linear camera response curve, shot noise, etc.
*/
class CameraSimulator
{
public:
class CameraSimulator {
public:
CameraSimulator(double exposure_time_ms)
: exposure_time_(ze::secToNanosec(exposure_time_ms / 1000.0))
{
: exposure_time_(ze::secToNanosec(exposure_time_ms / 1000.0)) {
buffer_.reset(new ImageBuffer(exposure_time_));
}
bool imageCallback(const Image& img, Time time,
const ImagePtr &camera_image);
bool imageCallback(
const Image& img, Time time, const ImagePtr& camera_image
);
private:
private:
ImageBuffer::Ptr buffer_;
const Duration exposure_time_;
};
};
} // namespace event_camera_simulator

18
event_camera_simulator/esim/include/esim/esim/event_simulator.hpp
View File

@ -4,7 +4,7 @@
namespace event_camera_simulator {
/*
/*
* The EventSimulator takes as input a sequence of stamped images,
* assumed to be sampled at a "sufficiently high" framerate,
* and simulates the principle of operation of an idea event camera
@ -14,12 +14,9 @@ namespace event_camera_simulator {
* The pixel-wise voltages are reset with the values from the first image
* which is passed to the simulator.
*/
class EventSimulator
{
public:
struct Config
{
class EventSimulator {
public:
struct Config {
double Cp;
double Cm;
double sigma_Cp;
@ -34,13 +31,12 @@ public:
EventSimulator(const Config& config)
: config_(config),
is_initialized_(false),
current_time_(0)
{}
current_time_(0) {}
void init(const Image& img, Time time);
Events imageCallback(const Image& img, Time time);
private:
private:
bool is_initialized_;
Time current_time_;
Image ref_values_;
@ -49,6 +45,6 @@ private:
cv::Size size_;
Config config_;
};
};
} // namespace event_camera_simulator

46
event_camera_simulator/esim/include/esim/esim/simulator.hpp
View File

@ -1,30 +1,29 @@
#pragma once
#include <esim/esim/event_simulator.hpp>
#include <esim/esim/camera_simulator.hpp>
#include <esim/esim/event_simulator.hpp>
#include <esim/visualization/publisher_interface.hpp>
namespace event_camera_simulator {
/* The Simulator forwards the simulated images / depth maps
* from the data provider to multiple, specialized camera simulators, such as:
* (i) event camera simulators that simulate events based on sequences of images
* (ii) camera simulators that simulate real cameras
* (including motion blur, camera response function, noise, etc.)
/* The Simulator forwards the simulated images / depth maps
* from the data provider to multiple, specialized camera simulators, such
* as: (i) event camera simulators that simulate events based on sequences
* of images (ii) camera simulators that simulate real cameras (including
* motion blur, camera response function, noise, etc.)
*
* The Simulator then forwards the simulated data to one or more publishers.
*/
class Simulator
{
public:
Simulator(size_t num_cameras,
*/
class Simulator {
public:
Simulator(
size_t num_cameras,
const EventSimulator::Config& event_sim_config,
double exposure_time_ms)
double exposure_time_ms
)
: num_cameras_(num_cameras),
exposure_time_(ze::millisecToNanosec(exposure_time_ms))
{
for(size_t i=0; i<num_cameras_; ++i)
{
exposure_time_(ze::millisecToNanosec(exposure_time_ms)) {
for (size_t i = 0; i < num_cameras_; ++i) {
event_simulators_.push_back(EventSimulator(event_sim_config));
camera_simulators_.push_back(CameraSimulator(exposure_time_ms));
}
@ -32,20 +31,21 @@ public:
~Simulator();
void addPublisher(const Publisher::Ptr& publisher)
{
void addPublisher(const Publisher::Ptr& publisher) {
CHECK(publisher);
publishers_.push_back(std::move(publisher));
}
void dataProviderCallback(const SimulatorData& sim_data);
void publishData(const SimulatorData &sim_data,
const EventsVector &events,
void publishData(
const SimulatorData& sim_data,
const EventsVector& events,
bool camera_simulator_success,
const ImagePtrVector &camera_images);
const ImagePtrVector& camera_images
);
private:
private:
size_t num_cameras_;
std::vector<EventSimulator> event_simulators_;
@ -55,6 +55,6 @@ private:
std::vector<Publisher::Ptr> publishers_;
ImagePtrVector corrupted_camera_images_;
};
};
} // namespace event_camera_simulator

1
event_camera_simulator/esim/src/camera_simulator.cpp
View File

@ -88,7 +88,6 @@ namespace event_camera_simulator {
<< buffer_->getExposureTime() << std::endl;
exposures_file_.close();
*camera_image /= denom;
cv::Mat disp;
camera_image->convertTo(disp, CV_8U, 255);

104
event_camera_simulator/esim/src/event_simulator.cpp
View File

@ -1,40 +1,41 @@
#include <esim/esim/event_simulator.hpp>
#include <ze/common/random.hpp>
#include <glog/logging.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/common/random.hpp>
#include <ze/common/time_conversions.hpp>
namespace event_camera_simulator {
void EventSimulator::init(const Image &img, Time time)
{
VLOG(1) << "Initialized event camera simulator with sensor size: " << img.size();
VLOG(1) << "and contrast thresholds: C+ = " << config_.Cp << " , C- = " << config_.Cm;
void EventSimulator::init(const Image& img, Time time) {
VLOG(1) << "Initialized event camera simulator with sensor size: "
<< img.size();
VLOG(1) << "and contrast thresholds: C+ = " << config_.Cp
<< " , C- = " << config_.Cm;
is_initialized_ = true;
last_img_ = img.clone();
ref_values_ = img.clone();
last_event_timestamp_ = TimestampImage::zeros(img.size());
current_time_ = time;
size_ = img.size();
}
}
Events EventSimulator::imageCallback(const Image& img, Time time)
{
Events EventSimulator::imageCallback(const Image& img, Time time) {
CHECK_GE(time, 0);
Image preprocessed_img = img.clone();
if(config_.use_log_image)
{
LOG_FIRST_N(INFO, 1) << "Converting the image to log image with eps = " << config_.log_eps << ".";
if (config_.use_log_image) {
LOG_FIRST_N(INFO, 1)
<< "Converting the image to log image with eps = "
<< config_.log_eps << ".";
cv::log(config_.log_eps + img, preprocessed_img);
}
if(!is_initialized_)
{
if (!is_initialized_) {
init(preprocessed_img, time);
return {};
}
// For each pixel, check if new events need to be generated since the last image sample
// For each pixel, check if new events need to be generated since the
// last image sample
static constexpr ImageFloatType tolerance = 1e-6;
Events events;
Duration delta_t_ns = time - current_time_;
@ -42,58 +43,61 @@ Events EventSimulator::imageCallback(const Image& img, Time time)
CHECK_GT(delta_t_ns, 0u);
CHECK_EQ(img.size(), size_);
for (int y = 0; y < size_.height; ++y)
{
for (int x = 0; x < size_.width; ++x)
{
for (int y = 0; y < size_.height; ++y) {
for (int x = 0; x < size_.width; ++x) {
ImageFloatType itdt = preprocessed_img(y, x);
ImageFloatType it = last_img_(y, x);
ImageFloatType prev_cross = ref_values_(y, x);
if (std::fabs (it - itdt) > tolerance)
{
if (std::fabs(it - itdt) > tolerance) {
ImageFloatType pol = (itdt >= it) ? +1.0 : -1.0;
ImageFloatType C = (pol > 0) ? config_.Cp : config_.Cm;
ImageFloatType sigma_C = (pol > 0) ? config_.sigma_Cp : config_.sigma_Cm;
if(sigma_C > 0)
{
C += ze::sampleNormalDistribution<ImageFloatType>(false, 0, sigma_C);
constexpr ImageFloatType minimum_contrast_threshold = 0.01;
ImageFloatType sigma_C =
(pol > 0) ? config_.sigma_Cp : config_.sigma_Cm;
if (sigma_C > 0) {
C += ze::sampleNormalDistribution<ImageFloatType>(
false,
0,
sigma_C
);
constexpr ImageFloatType minimum_contrast_threshold =
0.01;
C = std::max(minimum_contrast_threshold, C);
}
ImageFloatType curr_cross = prev_cross;
bool all_crossings = false;
do
{
do {
curr_cross += pol * C;
if ((pol > 0 && curr_cross > it && curr_cross <= itdt)
|| (pol < 0 && curr_cross < it && curr_cross >= itdt))
{
Duration edt = (curr_cross - it) * delta_t_ns / (itdt - it);
|| (pol < 0 && curr_cross < it && curr_cross >= itdt
)) {
Duration edt =
(curr_cross - it) * delta_t_ns / (itdt - it);
Time t = current_time_ + edt;
// check that pixel (x,y) is not currently in a "refractory" state
// i.e. |t-that last_timestamp(x,y)| >= refractory_period
const Time last_stamp_at_xy = ze::secToNanosec(last_event_timestamp_(y,x));
// check that pixel (x,y) is not currently in a
// "refractory" state i.e. |t-that
// last_timestamp(x,y)| >= refractory_period
const Time last_stamp_at_xy =
ze::secToNanosec(last_event_timestamp_(y, x));
CHECK_GE(t, last_stamp_at_xy);
const Duration dt = t - last_stamp_at_xy;
if(last_event_timestamp_(y,x) == 0 || dt >= config_.refractory_period_ns)
{
if (last_event_timestamp_(y, x) == 0
|| dt >= config_.refractory_period_ns) {
events.push_back(Event(x, y, t, pol > 0));
last_event_timestamp_(y,x) = ze::nanosecToSecTrunc(t);
}
else
{
VLOG(3) << "Dropping event because time since last event ("
last_event_timestamp_(y, x) =
ze::nanosecToSecTrunc(t);
} else {
VLOG(3)
<< "Dropping event because time since last "
"event ("
<< dt << " ns) < refractory period ("
<< config_.refractory_period_ns << " ns).";
}
ref_values_(y,x) = curr_cross;
}
else
{
ref_values_(y, x) = curr_cross;
} else {
all_crossings = true;
}
} while (!all_crossings);
@ -107,13 +111,13 @@ Events EventSimulator::imageCallback(const Image& img, Time time)
// Sort the events by increasing timestamps, since this is what
// most event processing algorithms expect
sort(events.begin(), events.end(),
[](const Event& a, const Event& b) -> bool
{
return a.t < b.t;
});
sort(
events.begin(),
events.end(),
[](const Event& a, const Event& b) -> bool { return a.t < b.t; }
);
return events;
}
}
} // namespace event_camera_simulator

171
event_camera_simulator/esim/src/simulator.cpp
View File

@ -1,70 +1,92 @@
#include <esim/common/utils.hpp>
#include <esim/esim/simulator.hpp>
#include <ze/common/timer_collection.hpp>
#include <esim/common/utils.hpp>
namespace event_camera_simulator {
DECLARE_TIMER(TimerEventSimulator, timers_event_simulator_,
DECLARE_TIMER(
TimerEventSimulator,
timers_event_simulator_,
simulate_events,
visualization
);
Simulator::~Simulator()
{
Simulator::~Simulator() {
timers_event_simulator_.saveToFile("/tmp", "event_simulator.csv");
}
}
void Simulator::dataProviderCallback(const SimulatorData &sim_data)
{
void Simulator::dataProviderCallback(const SimulatorData& sim_data) {
CHECK_EQ(event_simulators_.size(), num_cameras_);
bool camera_simulator_success;
if(sim_data.images_updated)
{
if (sim_data.images_updated) {
EventsVector events(num_cameras_);
Time time = sim_data.timestamp;
// simulate the events and camera images for every sensor in the rig
{
auto t = timers_event_simulator_[TimerEventSimulator::simulate_events].timeScope();
for(size_t i=0; i<num_cameras_; ++i)
{
events[i] = event_simulators_[i].imageCallback(*sim_data.images[i], time);
auto t = timers_event_simulator_
[TimerEventSimulator::simulate_events]
.timeScope();
for (size_t i = 0; i < num_cameras_; ++i) {
events[i] = event_simulators_[i].imageCallback(
*sim_data.images[i],
time
);
if(corrupted_camera_images_.size() < num_cameras_)
{
// allocate memory for the corrupted camera images and set them to 0
corrupted_camera_images_.emplace_back(std::make_shared<Image>(sim_data.images[i]->size()));
if (corrupted_camera_images_.size() < num_cameras_) {
// allocate memory for the corrupted camera images and
// set them to 0
corrupted_camera_images_.emplace_back(
std::make_shared<Image>(sim_data.images[i]->size())
);
corrupted_camera_images_[i]->setTo(0.);
}
camera_simulator_success = camera_simulators_[i].imageCallback(*sim_data.images[i], time, corrupted_camera_images_[i]);
camera_simulator_success =
camera_simulators_[i].imageCallback(
*sim_data.images[i],
time,
corrupted_camera_images_[i]
);
}
}
// publish the simulation data + events
{
auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope();
publishData(sim_data, events, camera_simulator_success, corrupted_camera_images_);
auto t =
timers_event_simulator_[TimerEventSimulator::visualization]
.timeScope();
publishData(
sim_data,
events,
camera_simulator_success,
corrupted_camera_images_
);
}
}
else
{
} else {
{
// just forward the simulation data to the publisher
auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope();
publishData(sim_data, {}, camera_simulator_success, corrupted_camera_images_);
auto t =
timers_event_simulator_[TimerEventSimulator::visualization]
.timeScope();
publishData(
sim_data,
{},
camera_simulator_success,
corrupted_camera_images_
);
}
}
}
}
void Simulator::publishData(const SimulatorData& sim_data,
void Simulator::publishData(
const SimulatorData& sim_data,
const EventsVector& events,
bool camera_simulator_success,
const ImagePtrVector& camera_images)
{
if(publishers_.empty())
{
const ImagePtrVector& camera_images
) {
if (publishers_.empty()) {
LOG_FIRST_N(WARNING, 1) << "No publisher available";
return;
}
@ -74,70 +96,65 @@ void Simulator::publishData(const SimulatorData& sim_data,
const TransformationVector& T_W_Cs = sim_data.groundtruth.T_W_Cs;
const ze::CameraRig::Ptr& camera_rig = sim_data.camera_rig;
// Publish the new data (events, images, depth maps, poses, point clouds, etc.)
if(!events.empty())
{
for(const Publisher::Ptr& publisher : publishers_)
// Publish the new data (events, images, depth maps, poses, point
// clouds, etc.)
if (!events.empty())
for (const Publisher::Ptr& publisher : publishers_)
publisher->eventsCallback(events);
}
if(sim_data.poses_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
if (sim_data.poses_updated)
for (const Publisher::Ptr& publisher : publishers_)
publisher->poseCallback(T_W_B, T_W_Cs, time);
}
if(sim_data.twists_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
publisher->twistCallback(sim_data.groundtruth.angular_velocities_,
if (sim_data.twists_updated)
for (const Publisher::Ptr& publisher : publishers_)
publisher->twistCallback(
sim_data.groundtruth.angular_velocities_,
sim_data.groundtruth.linear_velocities_,
time);
}
if(sim_data.imu_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
publisher->imuCallback(sim_data.specific_force_corrupted, sim_data.angular_velocity_corrupted, time);
}
time
);
if (sim_data.imu_updated)
for (const Publisher::Ptr& publisher : publishers_)
publisher->imuCallback(
sim_data.specific_force_corrupted,
sim_data.angular_velocity_corrupted,
time
);
if(camera_rig)
{
for(const Publisher::Ptr& publisher : publishers_)
if (camera_rig)
for (const Publisher::Ptr& publisher : publishers_)
publisher->cameraInfoCallback(camera_rig, time);
}
if(sim_data.images_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
{
if (sim_data.images_updated) {
for (const Publisher::Ptr& publisher : publishers_) {
publisher->imageCallback(sim_data.images, time);
if(camera_simulator_success && time >= exposure_time_)
{
if (camera_simulator_success && time >= exposure_time_) {
// the images should be timestamped at mid-exposure
const Time mid_exposure_time = time - 0.5 * exposure_time_;
publisher->imageCorruptedCallback(camera_images, mid_exposure_time);
publisher->imageCorruptedCallback(
camera_images,
mid_exposure_time
);
}
}
}
if(sim_data.depthmaps_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
if (sim_data.depthmaps_updated)
for (const Publisher::Ptr& publisher : publishers_)
publisher->depthmapCallback(sim_data.depthmaps, time);
}
if(sim_data.optic_flows_updated)
{
for(const Publisher::Ptr& publisher : publishers_)
if (sim_data.optic_flows_updated)
for (const Publisher::Ptr& publisher : publishers_)
publisher->opticFlowCallback(sim_data.optic_flows, time);
}
if(sim_data.depthmaps_updated && !events.empty())
{
if (sim_data.depthmaps_updated && !events.empty()) {
PointCloudVector pointclouds(num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(sim_data.depthmaps[i]);
pointclouds[i] = eventsToPointCloud(events[i], *sim_data.depthmaps[i], camera_rig->atShared(i));
pointclouds[i] = eventsToPointCloud(
events[i],
*sim_data.depthmaps[i],
camera_rig->atShared(i)
);
}
for(const Publisher::Ptr& publisher : publishers_)
for (const Publisher::Ptr& publisher : publishers_)
publisher->pointcloudCallback(pointclouds, time);
}
}
}
} // namespace event_camera_simulator

120
event_camera_simulator/esim/test/test_event_simulator.cpp
View File

@ -1,68 +1,63 @@
#include <esim/esim/event_simulator.hpp>
#include <ze/common/test_entrypoint.hpp>
#include <fstream>
#include <opencv2/highgui/highgui.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/string_utils.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/test_entrypoint.hpp>
#include <ze/common/time_conversions.hpp>
#include <ze/matplotlib/matplotlibcpp.hpp>
#include <opencv2/highgui/highgui.hpp>
#define USE_OPENCV
namespace event_camera_simulator
{
namespace event_camera_simulator {
class CSVImageLoader
{
public:
class CSVImageLoader {
public:
CSVImageLoader(const std::string& path_to_data_folder)
: path_to_data_folder_(path_to_data_folder)
{
images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv"));
: path_to_data_folder_(path_to_data_folder) {
images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv")
);
CHECK(images_in_str_.is_open());
}
bool next(int64_t& stamp, Image& img)
{
bool next(int64_t& stamp, Image& img) {
std::string line;
if(!getline(images_in_str_, line))
{
if (!getline(images_in_str_, line)) {
LOG(INFO) << "Finished reading all the images in the folder";
return false;
}
if('%' != line.at(0) && '#' != line.at(0))
{
std::vector<std::string> items = ze::splitString(line, delimiter_);
if ('%' != line.at(0) && '#' != line.at(0)) {
std::vector<std::string> items =
ze::splitString(line, delimiter_);
stamp = std::stoll(items[0]);
const std::string& path_to_image
= ze::joinPath(path_to_data_folder_, "frame", "cam_0", items[1]);
const std::string& path_to_image = ze::joinPath(
path_to_data_folder_,
"frame",
"cam_0",
items[1]
);
img = cv::imread(path_to_image, 0);
CHECK(img.data) << "Error loading image: " << path_to_image;
return true;
}
else
{
} else {
return next(stamp, img);
}
}
private:
private:
std::ifstream images_in_str_;
const char delimiter_{','};
std::string path_to_data_folder_;
};
};
} // namespace event_camera_simulator
std::string getTestDataDir(const std::string& dataset_name)
{
std::string getTestDataDir(const std::string& dataset_name) {
using namespace ze;
const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH");
@ -77,8 +72,7 @@ std::string getTestDataDir(const std::string& dataset_name)
return path;
}
TEST(EventSimulator, testImageReconstruction)
{
TEST(EventSimulator, testImageReconstruction) {
using namespace event_camera_simulator;
// Load image sequence from folder
@ -94,22 +88,20 @@ TEST(EventSimulator, testImageReconstruction)
event_sim_config.log_eps = 0.001;
EventSimulator simulator(event_sim_config);
LOG(INFO) << "Testing event camera simulator with C+ = " << event_sim_config.Cp
<< ", C- = " << event_sim_config.Cm;
LOG(INFO) << "Testing event camera simulator with C+ = "
<< event_sim_config.Cp << ", C- = " << event_sim_config.Cm;
const ImageFloatType max_reconstruction_error
= std::max(event_sim_config.Cp, event_sim_config.Cm);
const ImageFloatType max_reconstruction_error =
std::max(event_sim_config.Cp, event_sim_config.Cm);
bool is_first_image = true;
Image I, L, L_reconstructed;
int64_t stamp;
while(reader.next(stamp, I))
{
I.convertTo(I, cv::DataType<ImageFloatType>::type, 1./255.);
while (reader.next(stamp, I)) {
I.convertTo(I, cv::DataType<ImageFloatType>::type, 1. / 255.);
cv::log(event_sim_config.log_eps + I, L);
if(is_first_image)
{
if (is_first_image) {
// Initialize reconstructed image from the ground truth image
L_reconstructed = L.clone();
is_first_image = false;
@ -118,19 +110,19 @@ TEST(EventSimulator, testImageReconstruction)
Events events = simulator.imageCallback(I, stamp);
// Reconstruct next image from previous one using the events in between
for(const Event& e : events)
{
for (const Event& e : events) {
ImageFloatType pol = e.pol ? 1. : -1.;
const ImageFloatType C = e.pol ? event_sim_config.Cp : event_sim_config.Cm;
L_reconstructed(e.y,e.x) += pol * C;
const ImageFloatType C =
e.pol ? event_sim_config.Cp : event_sim_config.Cm;
L_reconstructed(e.y, e.x) += pol * C;
}
// Check that the reconstruction error is bounded by the contrast thresholds
for(int y=0; y<I.rows; ++y)
{
for(int x=0; x<I.cols; ++x)
{
const ImageFloatType reconstruction_error = std::fabs(L_reconstructed(y,x) - L(y,x));
// Check that the reconstruction error is bounded by the contrast
// thresholds
for (int y = 0; y < I.rows; ++y) {
for (int x = 0; x < I.cols; ++x) {
const ImageFloatType reconstruction_error =
std::fabs(L_reconstructed(y, x) - L(y, x));
VLOG_EVERY_N(2, I.rows * I.cols) << reconstruction_error;
EXPECT_LE(reconstruction_error, max_reconstruction_error);
}
@ -147,9 +139,7 @@ TEST(EventSimulator, testImageReconstruction)
}
}
TEST(EventSimulator, testEvolutionReconstructionError)
{
TEST(EventSimulator, testEvolutionReconstructionError) {
using namespace event_camera_simulator;
// Load image sequence from folder
@ -166,21 +156,19 @@ TEST(EventSimulator, testEvolutionReconstructionError)
EventSimulator simulator(event_sim_config);
const double contrast_bias = 0.0;
LOG(INFO) << "Testing event camera simulator with C+ = " << event_sim_config.Cp
<< ", C- = " << event_sim_config.Cm;
LOG(INFO) << "Testing event camera simulator with C+ = "
<< event_sim_config.Cp << ", C- = " << event_sim_config.Cm;
std::vector<ze::real_t> times, mean_errors, stddev_errors;
bool is_first_image = true;
Image I, L, L_reconstructed;
int64_t stamp;
while(reader.next(stamp, I))
{
while (reader.next(stamp, I)) {
LOG_EVERY_N(INFO, 50) << "t = " << ze::nanosecToSecTrunc(stamp) << " s";
I.convertTo(I, cv::DataType<ImageFloatType>::type, 1./255.);
I.convertTo(I, cv::DataType<ImageFloatType>::type, 1. / 255.);
cv::log(event_sim_config.log_eps + I, L);
if(is_first_image)
{
if (is_first_image) {
// Initialize reconstructed image from the ground truth image
L_reconstructed = L.clone();
is_first_image = false;
@ -189,21 +177,21 @@ TEST(EventSimulator, testEvolutionReconstructionError)
Events events = simulator.imageCallback(I, stamp);
// Reconstruct next image from previous one using the events in between
for(const Event& e : events)
{
for (const Event& e : events) {
ImageFloatType pol = e.pol ? 1. : -1.;
ImageFloatType C = e.pol ? event_sim_config.Cp : event_sim_config.Cm;
ImageFloatType C =
e.pol ? event_sim_config.Cp : event_sim_config.Cm;
C += contrast_bias;
L_reconstructed(e.y,e.x) += pol * C;
L_reconstructed(e.y, e.x) += pol * C;
}
// Compute the mean and average reconstruction error over the whole image
// Compute the mean and average reconstruction error over the whole
// image
Image error;
cv::absdiff(L, L_reconstructed, error);
cv::Scalar mean_error, stddev_error;
cv::meanStdDev(error, mean_error, stddev_error);
VLOG(1) << "Mean error: " << mean_error
<< ", Stddev: " << stddev_error;
VLOG(1) << "Mean error: " << mean_error << ", Stddev: " << stddev_error;
times.push_back(ze::nanosecToSecTrunc(stamp));
mean_errors.push_back(mean_error[0]);

119
event_camera_simulator/esim_common/include/esim/common/types.hpp
View File

@ -1,88 +1,83 @@
#pragma once
#include <ze/common/transformation.hpp>
#include <memory>
#include <opencv2/core/core.hpp>
#include <ze/cameras/camera_rig.hpp>
#include <memory>
#include <ze/common/transformation.hpp>
// FloatType defines the floating point accuracy (single or double precision)
// for the geometric operations (computing rotation matrices, point projection, etc.).
// This should typically be double precision (highest accuracy).
// for the geometric operations (computing rotation matrices, point projection,
// etc.). This should typically be double precision (highest accuracy).
#define FloatType ze::real_t
// ImageFloatType defines the floating point accuracy (single or double precision)
// of the intensity images (and depth images).
// Single precision should be enough there in most cases.
// ImageFloatType defines the floating point accuracy (single or double
// precision) of the intensity images (and depth images). Single precision
// should be enough there in most cases.
#define ImageFloatType float
namespace event_camera_simulator {
using Translation = ze::Position;
using Vector3 = ze::Vector3;
using Vector4 = ze::Vector4;
using Vector3i = Eigen::Vector3i;
using Translation = ze::Position;
using Vector3 = ze::Vector3;
using Vector4 = ze::Vector4;
using Vector3i = Eigen::Vector3i;
using Transformation = ze::Transformation;
using TransformationVector = ze::TransformationVector;
using TransformationPtr = std::shared_ptr<Transformation>;
using Transformation = ze::Transformation;
using TransformationVector = ze::TransformationVector;
using TransformationPtr = std::shared_ptr<Transformation>;
using Normal = ze::Vector3;
using CalibrationMatrix = ze::Matrix3;
using RotationMatrix = ze::Matrix3;
using HomographyMatrix = ze::Matrix3;
using Normal = ze::Vector3;
using CalibrationMatrix = ze::Matrix3;
using RotationMatrix = ze::Matrix3;
using HomographyMatrix = ze::Matrix3;
using AngularVelocity = ze::Vector3;
using LinearVelocity = ze::Vector3;
using AngularVelocityVector = std::vector<AngularVelocity>;
using LinearVelocityVector = std::vector<LinearVelocity>;
using AngularVelocity = ze::Vector3;
using LinearVelocity = ze::Vector3;
using AngularVelocityVector = std::vector<AngularVelocity>;
using LinearVelocityVector = std::vector<LinearVelocity>;
using uint16_t = ze::uint16_t;
using uint16_t = ze::uint16_t;
using Time = ze::int64_t;
using Duration = ze::uint64_t;
using Image = cv::Mat_<ImageFloatType>;
using ImagePtr = std::shared_ptr<Image>;
using Depthmap = cv::Mat_<ImageFloatType>;
using OpticFlow = cv::Mat_< cv::Vec<ImageFloatType, 2> >;
using OpticFlowPtr = std::shared_ptr<OpticFlow>;
using DepthmapPtr = std::shared_ptr<Depthmap>;
using Time = ze::int64_t;
using Duration = ze::uint64_t;
using Image = cv::Mat_<ImageFloatType>;
using ImagePtr = std::shared_ptr<Image>;
using Depthmap = cv::Mat_<ImageFloatType>;
using OpticFlow = cv::Mat_<cv::Vec<ImageFloatType, 2>>;
using OpticFlowPtr = std::shared_ptr<OpticFlow>;
using DepthmapPtr = std::shared_ptr<Depthmap>;
using ImagePtrVector = std::vector<ImagePtr>;
using DepthmapPtrVector = std::vector<DepthmapPtr>;
using OpticFlowPtrVector = std::vector<OpticFlowPtr>;
using ImagePtrVector = std::vector<ImagePtr>;
using DepthmapPtrVector = std::vector<DepthmapPtr>;
using OpticFlowPtrVector = std::vector<OpticFlowPtr>;
using Camera = ze::Camera;
using Camera = ze::Camera;
struct Event
{
struct Event {
Event(uint16_t x, uint16_t y, Time t, bool pol)
: x(x),
y(y),
t(t),
pol(pol)
{
}
pol(pol) {}
uint16_t x;
uint16_t y;
Time t;
bool pol;
};
};
using Events = std::vector<Event>;
using EventsVector = std::vector<Events>;
using EventsPtr = std::shared_ptr<Events>;
using Events = std::vector<Event>;
using EventsVector = std::vector<Events>;
using EventsPtr = std::shared_ptr<Events>;
struct PointXYZRGB
{
PointXYZRGB(FloatType x, FloatType y, FloatType z,
int red, int green, int blue)
struct PointXYZRGB {
PointXYZRGB(
FloatType x, FloatType y, FloatType z, int red, int green, int blue
)
: xyz(x, y, z),
rgb(red, green, blue) {}
PointXYZRGB(const Vector3& xyz)
: xyz(xyz) {}
PointXYZRGB(const Vector3& xyz): xyz(xyz) {}
PointXYZRGB(const Vector3& xyz, const Vector3i& rgb)
: xyz(xyz),
@ -90,13 +85,12 @@ struct PointXYZRGB
Vector3 xyz;
Vector3i rgb;
};
};
using PointCloud = std::vector<PointXYZRGB>;
using PointCloudVector = std::vector<PointCloud>;
using PointCloud = std::vector<PointXYZRGB>;
using PointCloudVector = std::vector<PointCloud>;
struct SimulatorData
{
struct SimulatorData {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//! Nanosecond timestamp.
@ -118,12 +112,12 @@ struct SimulatorData
//! corrupted by noise and bias.
Vector3 specific_force_corrupted;
//! The angular velocity (in the body frame) corrupted by noise and bias.
//! The angular velocity (in the body frame) corrupted by noise and
//! bias.
Vector3 angular_velocity_corrupted;
//! Groundtruth states.
struct Groundtruth
{
struct Groundtruth {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//! Pose of the body (i.e. the IMU) expressed in the world frame.
@ -136,8 +130,8 @@ struct SimulatorData
//! Poses of the cameras in the rig expressed in the world frame.
TransformationVector T_W_Cs;
//! Linear and angular velocities (i.e. twists) of the cameras in the rig,
//! expressed in each camera's local coordinate frame.
//! Linear and angular velocities (i.e. twists) of the cameras in
//! the rig, expressed in each camera's local coordinate frame.
LinearVelocityVector linear_velocities_;
AngularVelocityVector angular_velocities_;
@ -146,6 +140,7 @@ struct SimulatorData
std::vector<LinearVelocity> linear_velocity_obj_;
std::vector<AngularVelocity> angular_velocity_obj_;
};
Groundtruth groundtruth;
// Flags to indicate whether a value has been updated or not
@ -155,6 +150,6 @@ struct SimulatorData
bool twists_updated;
bool poses_updated;
bool imu_updated;
};
};
} // namespace event_camera_simulator

65
event_camera_simulator/esim_common/include/esim/common/utils.hpp
View File

@ -8,52 +8,65 @@ namespace ze {
namespace event_camera_simulator {
inline double degToRad(double deg)
{
inline double degToRad(double deg) {
return deg * CV_PI / 180.0;
}
}
inline double hfovToFocalLength(double hfov_deg, int W)
{
return 0.5 * static_cast<double>(W) / std::tan(0.5 * degToRad(hfov_deg));
}
inline double hfovToFocalLength(double hfov_deg, int W) {
return 0.5 * static_cast<double>(W)
/ std::tan(0.5 * degToRad(hfov_deg));
}
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera);
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera);
PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, const ze::Camera::Ptr& camera);
PointCloud eventsToPointCloud(
const Events& events,
const Depthmap& depthmap,
const ze::Camera::Ptr& camera
);
FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow);
FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow);
FloatType maxPredictedAbsBrightnessChange(const ImagePtr& I, const OpticFlowPtr& flow);
FloatType maxPredictedAbsBrightnessChange(
const ImagePtr& I, const OpticFlowPtr& flow
);
void gaussianBlur(ImagePtr& I, FloatType sigma);
void gaussianBlur(ImagePtr& I, FloatType sigma);
// Helper class to compute optic flow from a twist vector and depth map
// Precomputes a lookup table for pixel -> bearing vector correspondences
// to accelerate the computation
class OpticFlowHelper
{
public:
// Helper class to compute optic flow from a twist vector and depth map
// Precomputes a lookup table for pixel -> bearing vector correspondences
// to accelerate the computation
class OpticFlowHelper {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ZE_POINTER_TYPEDEFS(OpticFlowHelper);
OpticFlowHelper(const ze::Camera::Ptr& camera);
void computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC,
const DepthmapPtr& depthmap, OpticFlowPtr& flow);
void computeFromDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
OpticFlowPtr& flow
);
void computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow);
private:
void computeFromDepthCamTwistAndObjDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ,
const ze::Vector3& w_WOBJ,
const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow
);
private:
void precomputePixelToBearingLookupTable();
ze::Camera::Ptr camera_;
// Precomputed lookup table from keypoint -> bearing vector
ze::Bearings bearings_C_;
};
};
} // namespace event_camera_simulator

214
event_camera_simulator/esim_common/src/utils.cpp
View File

@ -1,67 +1,79 @@
#include <esim/common/utils.hpp>
#include <ze/cameras/camera_models.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/cameras/camera_models.hpp>
namespace event_camera_simulator {
OpticFlowHelper::OpticFlowHelper(const ze::Camera::Ptr& camera)
: camera_(camera)
{
OpticFlowHelper::OpticFlowHelper(const ze::Camera::Ptr& camera)
: camera_(camera) {
CHECK(camera_);
precomputePixelToBearingLookupTable();
}
}
void OpticFlowHelper::precomputePixelToBearingLookupTable()
{
// points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
void OpticFlowHelper::precomputePixelToBearingLookupTable() {
// points_C is a matrix containing the coordinates of each pixel
// coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
}
}
for (int y = 0; y < camera_->height(); ++y)
for (int x = 0; x < camera_->width(); ++x)
points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y);
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
}
void OpticFlowHelper::computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC,
const DepthmapPtr& depthmap, OpticFlowPtr& flow)
{
void OpticFlowHelper::computeFromDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
OpticFlowPtr& flow
) {
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
CHECK(depthmap->isContinuous());
const ze::Vector3 w_CW = -w_WC; // rotation speed of the world wrt the camera
const ze::Vector3 w_CW =
-w_WC; // rotation speed of the world wrt the camera
const ze::Vector3 v_CW = -v_WC; // speed of the world wrt the camera
const ze::Matrix33 R_CW = ze::skewSymmetric(w_CW);
Eigen::Map<const Eigen::Matrix<ImageFloatType, 1, Eigen::Dynamic, Eigen::RowMajor>> depths(depthmap->ptr<ImageFloatType>(), 1, depthmap->rows * depthmap->cols);
Eigen::Map<
const Eigen::
Matrix<ImageFloatType, 1, Eigen::Dynamic, Eigen::RowMajor>>
depths(
depthmap->ptr<ImageFloatType>(),
1,
depthmap->rows * depthmap->cols
);
ze::Positions Xs = bearings_C_;
Xs.array().rowwise() *= depths.cast<FloatType>().array();
ze::Matrix6X dproject_dX =
camera_->dProject_dLandmarkVectorized(Xs);
ze::Matrix6X dproject_dX = camera_->dProject_dLandmarkVectorized(Xs);
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const Vector3 X = Xs.col(x + camera_->width() * y);
ze::Matrix31 dXdt = R_CW * X + v_CW;
ze::Vector2 flow_vec
= Eigen::Map<ze::Matrix23>(dproject_dX.col(x + camera_->width() * y).data()) * dXdt;
ze::Vector2 flow_vec =
Eigen::Map<ze::Matrix23>(
dproject_dX.col(x + camera_->width() * y).data()
)
* dXdt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ, OpticFlowPtr& flow)
{
void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ,
const ze::Vector3& w_WOBJ,
const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow
) {
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
@ -70,112 +82,122 @@ void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vect
const ze::Matrix33 w_WC_tilde = ze::skewSymmetric(w_WC);
const ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(w_WOBJ);
Eigen::Map<const Eigen::Matrix<ImageFloatType, 1, Eigen::Dynamic, Eigen::RowMajor>> depths(depthmap->ptr<ImageFloatType>(), 1, depthmap->rows * depthmap->cols);
Eigen::Map<
const Eigen::
Matrix<ImageFloatType, 1, Eigen::Dynamic, Eigen::RowMajor>>
depths(
depthmap->ptr<ImageFloatType>(),
1,
depthmap->rows * depthmap->cols
);
ze::Positions Xs = bearings_C_;
Xs.array().rowwise() *= depths.cast<FloatType>().array();
ze::Matrix6X dproject_dX =
camera_->dProject_dLandmarkVectorized(Xs);
ze::Matrix6X dproject_dX = camera_->dProject_dLandmarkVectorized(Xs);
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const Vector3 r_CX = Xs.col(x + camera_->width() * y);
const Vector3 r_OBJX = r_CX - r_COBJ;
ze::Matrix31 dXdt = v_WOBJ - v_WC - w_WC_tilde*r_CX + w_WOBJ_tilde*r_OBJX;
ze::Vector2 flow_vec
= Eigen::Map<ze::Matrix23>(dproject_dX.col(x + camera_->width() * y).data()) * dXdt;
ze::Matrix31 dXdt =
v_WOBJ - v_WC - w_WC_tilde * r_CX + w_WOBJ_tilde * r_OBJX;
ze::Vector2 flow_vec =
Eigen::Map<ze::Matrix23>(
dproject_dX.col(x + camera_->width() * y).data()
)
* dXdt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow)
{
FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow) {
CHECK(flow);
FloatType max_squared_magnitude = 0;
for(int y=0; y<flow->rows; ++y)
{
for(int x=0; x<flow->cols; ++x)
{
const FloatType squared_magnitude = cv::norm((*flow)(y,x), cv::NORM_L2SQR);
if(squared_magnitude > max_squared_magnitude)
{
for (int y = 0; y < flow->rows; ++y) {
for (int x = 0; x < flow->cols; ++x) {
const FloatType squared_magnitude =
cv::norm((*flow)(y, x), cv::NORM_L2SQR);
if (squared_magnitude > max_squared_magnitude)
max_squared_magnitude = squared_magnitude;
}
}
}
return std::sqrt(max_squared_magnitude);
}
}
FloatType maxPredictedAbsBrightnessChange(const ImagePtr& I, const OpticFlowPtr& flow)
{
FloatType maxPredictedAbsBrightnessChange(
const ImagePtr& I, const OpticFlowPtr& flow
) {
const size_t height = I->rows;
const size_t width = I->cols;
Image Ix, Iy; // horizontal/vertical gradients of I
// the factor 1/8 accounts for the scaling introduced by the Sobel filter mask
//cv::Sobel(*I, Ix, cv::DataType<ImageFloatType>::type, 1, 0, 3, 1./8.);
//cv::Sobel(*I, Iy, cv::DataType<ImageFloatType>::type, 0, 1, 3, 1./8.);
// the factor 1/8 accounts for the scaling introduced by the Sobel
// filter mask cv::Sobel(*I, Ix, cv::DataType<ImageFloatType>::type, 1,
// 0, 3, 1./8.); cv::Sobel(*I, Iy, cv::DataType<ImageFloatType>::type,
// 0, 1, 3, 1./8.);
// the factor 1/32 accounts for the scaling introduced by the Scharr filter mask
cv::Scharr(*I, Ix, cv::DataType<ImageFloatType>::type, 1, 0, 1./32.);
cv::Scharr(*I, Iy, cv::DataType<ImageFloatType>::type, 0, 1, 1./32.);
// the factor 1/32 accounts for the scaling introduced by the Scharr
// filter mask
cv::Scharr(*I, Ix, cv::DataType<ImageFloatType>::type, 1, 0, 1. / 32.);
cv::Scharr(*I, Iy, cv::DataType<ImageFloatType>::type, 0, 1, 1. / 32.);
Image Lx, Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I * dI/dx
static const ImageFloatType eps = 1e-3; // small additive term to avoid problems at I=0
cv::divide(Ix, *I+eps, Lx);
cv::divide(Iy, *I+eps, Ly);
Image Lx,
Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I *
// dI/dx
static const ImageFloatType eps =
1e-3; // small additive term to avoid problems at I=0
cv::divide(Ix, *I + eps, Lx);
cv::divide(Iy, *I + eps, Ly);
Image dLdt(height, width);
for(int y=0; y<height; ++y)
{
for(int x=0; x<width; ++x)
{
for (int y = 0; y < height; ++y) {
for (int x = 0; x < width; ++x) {
// dL/dt ~= - <nablaL, flow>
const ImageFloatType dLdt_at_xy =
Lx(y,x) * (*flow)(y,x)[0] +
Ly(y,x) * (*flow)(y,x)[1]; // "-" sign ignored since we are interested in the absolute value...
dLdt(y,x) = std::fabs(dLdt_at_xy);
Lx(y, x) * (*flow)(y, x)[0]
+ Ly(y, x)
* (*flow)(
y,
x
)[1]; // "-" sign ignored since we are
// interested in the absolute value...
dLdt(y, x) = std::fabs(dLdt_at_xy);
}
}
double min_dLdt, max_dLdt;
int min_idx, max_idx;
cv::minMaxIdx(dLdt, &min_dLdt, &max_dLdt,
&min_idx, &max_idx);
cv::minMaxIdx(dLdt, &min_dLdt, &max_dLdt, &min_idx, &max_idx);
return static_cast<FloatType>(max_dLdt);
}
}
void gaussianBlur(ImagePtr& I, FloatType sigma)
{
cv::GaussianBlur(*I, *I, cv::Size(15,15), sigma, sigma);
}
void gaussianBlur(ImagePtr& I, FloatType sigma) {
cv::GaussianBlur(*I, *I, cv::Size(15, 15), sigma, sigma);
}
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera)
{
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera) {
CHECK(camera);
const ze::VectorX params = camera->projectionParameters();
CalibrationMatrix K;
K << params(0), 0, params(2),
0, params(1), params(3),
0, 0, 1;
K << params(0), 0, params(2), 0, params(1), params(3), 0, 0, 1;
return K;
}
}
PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, const ze::Camera::Ptr& camera)
{
PointCloud eventsToPointCloud(
const Events& events,
const Depthmap& depthmap,
const ze::Camera::Ptr& camera
) {
PointCloud pcl_camera;
for(const Event& ev : events)
{
Vector3 X_c = camera->backProject(ze::Keypoint(ev.x,ev.y));
for (const Event& ev : events) {
Vector3 X_c = camera->backProject(ze::Keypoint(ev.x, ev.y));
X_c[0] /= X_c[2];
X_c[1] /= X_c[2];
X_c[2] = 1.;
const ImageFloatType z = depthmap(ev.y,ev.x);
const ImageFloatType z = depthmap(ev.y, ev.x);
Vector3 P_c = z * X_c;
Vector3i rgb;
static const Vector3i red(255, 0, 0);
@ -185,6 +207,6 @@ PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, co
pcl_camera.push_back(P_c_intensity);
}
return pcl_camera;
}
}
} // namespace event_camera_simulator

179
event_camera_simulator/esim_common/test/test_utils.cpp
View File

@ -1,13 +1,12 @@
#include <esim/common/utils.hpp>
#include <ze/common/test_entrypoint.hpp>
#include <ze/cameras/camera_rig.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/string_utils.hpp>
#include <ze/cameras/camera_rig.hpp>
#include <ze/common/random.hpp>
#include <ze/common/string_utils.hpp>
#include <ze/common/test_entrypoint.hpp>
std::string getTestDataDir(const std::string& dataset_name)
{
std::string getTestDataDir(const std::string& dataset_name) {
using namespace ze;
const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH");
@ -24,28 +23,29 @@ std::string getTestDataDir(const std::string& dataset_name)
namespace event_camera_simulator {
void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
void computeOpticFlowFiniteDifference(
const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
OpticFlowPtr& flow)
{
OpticFlowPtr& flow
) {
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
const FloatType dt = 0.001;
for(int y=0; y<flow->rows; ++y)
{
for(int x=0; x<flow->cols; ++x)
{
ze::Keypoint u_t(x,y);
for (int y = 0; y < flow->rows; ++y) {
for (int x = 0; x < flow->cols; ++x) {
ze::Keypoint u_t(x, y);
ze::Bearing f = camera->backProject(u_t);
const FloatType z = static_cast<FloatType>((*depth)(y,x));
ze::Position Xc_t = z * ze::Position(f[0]/f[2], f[1]/f[2], 1.);
const FloatType z = static_cast<FloatType>((*depth)(y, x));
ze::Position Xc_t =
z * ze::Position(f[0] / f[2], f[1] / f[2], 1.);
ze::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Rotation dR =
ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = -linear_velocity * dt;
// Transform Xc(t) to Xc(t+dt)
@ -62,39 +62,43 @@ void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
VLOG(5) << u_t_dt;
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
void computeOpticFlowFiniteDifference(
const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
const ze::Vector3& r_COBJ,
const ze::Vector3& angular_velocity_obj,
const ze::Vector3& linear_velocity_obj,
OpticFlowPtr& flow)
{
OpticFlowPtr& flow
) {
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
const FloatType dt = 0.001;
for(int y=0; y<flow->rows; ++y)
{
for(int x=0; x<flow->cols; ++x)
{
ze::Keypoint u_t(x,y);
for (int y = 0; y < flow->rows; ++y) {
for (int x = 0; x < flow->cols; ++x) {
ze::Keypoint u_t(x, y);
ze::Bearing f = camera->backProject(u_t);
const FloatType z = static_cast<FloatType>((*depth)(y,x));
ze::Position Xc_t = z * ze::Position(f[0]/f[2], f[1]/f[2], 1.);
const FloatType z = static_cast<FloatType>((*depth)(y, x));
ze::Position Xc_t =
z * ze::Position(f[0] / f[2], f[1] / f[2], 1.);
ze::Position r_OBJX = Xc_t - r_COBJ;
ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(angular_velocity_obj);
ze::Matrix33 w_WOBJ_tilde =
ze::skewSymmetric(angular_velocity_obj);
ze::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = linear_velocity_obj*dt - linear_velocity * dt + w_WOBJ_tilde*r_OBJX*dt;
ze::Transformation::Rotation dR =
ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = linear_velocity_obj * dt
- linear_velocity * dt
+ w_WOBJ_tilde * r_OBJX * dt;
// Transform Xc(t) to Xc(t+dt)
ze::Transformation T_tdt_t;
@ -110,39 +114,37 @@ void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
VLOG(5) << u_t_dt;
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
} // event_camera_simulator
} // namespace event_camera_simulator
TEST(Utils, testOpticFlowComputation)
{
TEST(Utils, testOpticFlowComputation) {
using namespace event_camera_simulator;
// Load camera calib from folder
const std::string path_to_data_folder = getTestDataDir("camera_calibs");
ze::CameraRig::Ptr camera_rig
= ze::cameraRigFromYaml(ze::joinPath(path_to_data_folder, "pinhole_mono.yaml"));
ze::CameraRig::Ptr camera_rig = ze::cameraRigFromYaml(
ze::joinPath(path_to_data_folder, "pinhole_mono.yaml")
);
CHECK(camera_rig);
const ze::Camera::Ptr camera = camera_rig->atShared(0);
cv::Size sensor_size(camera->width(), camera->height());
OpticFlowPtr flow_analytic =
std::make_shared<OpticFlow>(sensor_size);
OpticFlowPtr flow_analytic = std::make_shared<OpticFlow>(sensor_size);
// Sample random depth map
const ImageFloatType z_mean = 5.0;
const ImageFloatType z_stddev = 0.5;
DepthmapPtr depth = std::make_shared<Depthmap>(sensor_size);
for(int y=0; y<sensor_size.height; ++y)
{
for(int x=0; x<sensor_size.width; ++x)
{
(*depth)(y,x) = ze::sampleNormalDistribution(true, z_mean, z_stddev);
for (int y = 0; y < sensor_size.height; ++y) {
for (int x = 0; x < sensor_size.width; ++x) {
(*depth)(y, x) =
ze::sampleNormalDistribution(true, z_mean, z_stddev);
}
}
@ -157,22 +159,36 @@ TEST(Utils, testOpticFlowComputation)
// Compute optic flow on the image plane according
// to the sampled angular/linear velocity
OpticFlowHelper optic_flow_helper(camera);
optic_flow_helper.computeFromDepthAndTwist(angular_velocity, linear_velocity,
depth, flow_analytic);
optic_flow_helper.computeFromDepthAndTwist(
angular_velocity,
linear_velocity,
depth,
flow_analytic
);
OpticFlowPtr flow_finite_diff =
std::make_shared<OpticFlow>(sensor_size);
OpticFlowPtr flow_finite_diff = std::make_shared<OpticFlow>(sensor_size);
computeOpticFlowFiniteDifference(camera, angular_velocity, linear_velocity,
depth, flow_finite_diff);
computeOpticFlowFiniteDifference(
camera,
angular_velocity,
linear_velocity,
depth,
flow_finite_diff
);
// Check that the analytical flow and finite-difference flow match
for(int y=0; y<sensor_size.height; ++y)
{
for(int x=0; x<sensor_size.width; ++x)
{
EXPECT_NEAR((*flow_analytic)(y,x)[0], (*flow_finite_diff)(y,x)[0], 0.1);
EXPECT_NEAR((*flow_analytic)(y,x)[1], (*flow_finite_diff)(y,x)[1], 0.1);
for (int y = 0; y < sensor_size.height; ++y) {
for (int x = 0; x < sensor_size.width; ++x) {
EXPECT_NEAR(
(*flow_analytic)(y, x)[0],
(*flow_finite_diff)(y, x)[0],
0.1
);
EXPECT_NEAR(
(*flow_analytic)(y, x)[1],
(*flow_finite_diff)(y, x)[1],
0.1
);
}
}
@ -183,7 +199,11 @@ TEST(Utils, testOpticFlowComputation)
// sample random obj position and linear and angular velocity
ze::Vector3 r_COBJ;
r_COBJ.setRandom();
r_COBJ(2) = ze::sampleNormalDistribution(true, z_mean, z_stddev); // assume object is in front of camera
r_COBJ(2) = ze::sampleNormalDistribution(
true,
z_mean,
z_stddev
); // assume object is in front of camera
ze::Vector3 angular_velocity_obj, linear_velocity_obj;
angular_velocity_obj.setRandom();
@ -191,21 +211,40 @@ TEST(Utils, testOpticFlowComputation)
// Compute optic flow on the image plane according
// to the sampled angular/linear velocity
optic_flow_helper.computeFromDepthCamTwistAndObjDepthAndTwist(angular_velocity, linear_velocity, depth,
r_COBJ, angular_velocity_obj, linear_velocity_obj,
flow_analytic);
optic_flow_helper.computeFromDepthCamTwistAndObjDepthAndTwist(
angular_velocity,
linear_velocity,
depth,
r_COBJ,
angular_velocity_obj,
linear_velocity_obj,
flow_analytic
);
computeOpticFlowFiniteDifference(camera, angular_velocity, linear_velocity, depth,
r_COBJ, angular_velocity_obj, linear_velocity_obj,
flow_finite_diff);
computeOpticFlowFiniteDifference(
camera,
angular_velocity,
linear_velocity,
depth,
r_COBJ,
angular_velocity_obj,
linear_velocity_obj,
flow_finite_diff
);
// Check that the analytical flow and finite-difference flow match
for(int y=0; y<sensor_size.height; ++y)
{
for(int x=0; x<sensor_size.width; ++x)
{
EXPECT_NEAR((*flow_analytic)(y,x)[0], (*flow_finite_diff)(y,x)[0], 0.1);
EXPECT_NEAR((*flow_analytic)(y,x)[1], (*flow_finite_diff)(y,x)[1], 0.1);
for (int y = 0; y < sensor_size.height; ++y) {
for (int x = 0; x < sensor_size.width; ++x) {
EXPECT_NEAR(
(*flow_analytic)(y, x)[0],
(*flow_finite_diff)(y, x)[0],
0.1
);
EXPECT_NEAR(
(*flow_analytic)(y, x)[1],
(*flow_finite_diff)(y, x)[1],
0.1
);
}
}
}

42
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_base.hpp
View File

@ -1,35 +1,28 @@
#pragma once
#include <atomic>
#include <esim/common/types.hpp>
#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;
class Mat;
}
namespace event_camera_simulator {
using Callback =
std::function<void (const SimulatorData& sim_data)>;
using Callback = std::function<void(const SimulatorData& sim_data)>;
enum class DataProviderType {
RendererOnline,
Folder,
Rosbag
};
enum class DataProviderType { RendererOnline, Folder, Rosbag };
//! A data provider registers to a data source and triggers callbacks when
//! new data is available.
class DataProviderBase : ze::Noncopyable
{
public:
//! 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;
@ -39,10 +32,12 @@ public:
//! Process all callbacks. Waits until callback is processed.
void spin();
//! Read next data field and process callback. Returns false when datatset finished.
//! 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.
//! False if there is no more data to process or there was a shutdown
//! signal.
virtual bool ok() const = 0;
//! Pause data provider.
@ -58,17 +53,20 @@ public:
virtual size_t numCameras() const = 0;
//! Returns the camera rig
ze::CameraRig::Ptr getCameraRig() { return camera_rig_; }
ze::CameraRig::Ptr getCameraRig() {
return camera_rig_;
}
protected:
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.
};
private:
ze::SimpleSigtermHandler
signal_handler_; //!< Sets running_ to false when Ctrl-C is pressed.
};
} // namespace event_camera_simulator

6
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp
View File

@ -1,10 +1,10 @@
#pragma once
#include <gflags/gflags.h>
#include <esim/data_provider/data_provider_base.hpp>
#include <gflags/gflags.h>
namespace event_camera_simulator {
DataProviderBase::Ptr loadDataProviderFromGflags();
DataProviderBase::Ptr loadDataProviderFromGflags();
} // namespace ze
} // namespace event_camera_simulator

17
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp
View File

@ -1,21 +1,19 @@
#pragma once
#include <esim/data_provider/data_provider_base.hpp>
#include <fstream>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <ze/cameras/camera_rig.hpp>
#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:
class DataProviderFromFolder : public DataProviderBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DataProviderFromFolder(const std::string& path_to_data_folder);
@ -28,8 +26,7 @@ public:
size_t numCameras() const override;
private:
private:
int64_t getTimeStamp(const std::string& ts_str) const;
std::string path_to_data_folder_;
@ -39,6 +36,6 @@ private:
bool finished_parsing_;
SimulatorData sim_data_;
};
};
} // namespace event_camera_simulator

46
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp
View File

@ -1,36 +1,35 @@
#pragma once
#include <esim/common/utils.hpp>
#include <esim/data_provider/data_provider_base.hpp>
#include <esim/rendering/renderer_base.hpp>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <ze/cameras/camera_rig.hpp>
#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>
#include <ze/vi_simulation/trajectory_simulator.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).
/**
* 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,
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);
ze::real_t simulation_adaptive_sampling_lambda
);
virtual ~DataProviderOnlineMoving3DCameraRig();
@ -40,8 +39,7 @@ public:
size_t numCameras() const override;
private:
private:
void updateGroundtruth();
void sampleImu();
void sampleFrame();
@ -57,9 +55,13 @@ private:
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 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_;
@ -70,6 +72,6 @@ private:
// dynamic objects
std::vector<ze::TrajectorySimulator::Ptr> trajectory_dyn_obj_;
};
};
} // namespace event_camera_simulator

30
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp
View File

@ -1,30 +1,29 @@
#pragma once
#include <esim/data_provider/data_provider_base.hpp>
#include <esim/rendering/simple_renderer_base.hpp>
#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,
class DataProviderOnlineSimple : public DataProviderBase {
public:
DataProviderOnlineSimple(
ze::real_t simulation_minimum_framerate,
int simulation_adaptive_sampling_method,
ze::real_t simulation_adaptive_sampling_lambda);
ze::real_t simulation_adaptive_sampling_lambda
);
virtual ~DataProviderOnlineSimple();
@ -34,7 +33,7 @@ public:
size_t numCameras() const override;
private:
private:
bool sampleFrame();
void setFrameUpdated();
@ -42,13 +41,16 @@ private:
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 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

36
event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp
View File

@ -1,30 +1,27 @@
// This file was adapted from the ze_oss project: https://github.com/zurich-eye/ze_oss/blob/master/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp
// This file was adapted from the ze_oss project:
// https://github.com/zurich-eye/ze_oss/blob/master/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp
// Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved
#pragma once
#include <esim/data_provider/data_provider_base.hpp>
#include <gflags/gflags.h>
#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>
#include <string>
#include <vector>
namespace event_camera_simulator {
class DataProviderRosbag : public DataProviderBase
{
public:
class DataProviderRosbag : public DataProviderBase {
public:
DataProviderRosbag(
const std::string& bag_filename,
const std::map<std::string, size_t>& camera_topics);
const std::map<std::string, size_t>& camera_topics
);
virtual ~DataProviderRosbag() = default;
@ -36,12 +33,14 @@ public:
size_t size() const;
private:
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);
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_;
@ -49,9 +48,10 @@ private:
int n_processed_images_ = 0;
// subscribed topics:
std::map<std::string, size_t> img_topic_camidx_map_; // camera_topic --> camera_id
std::map<std::string, size_t>
img_topic_camidx_map_; // camera_topic --> camera_id
SimulatorData sim_data_;
};
};
} // namespace event_camera_simulator

8
event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp
View File

@ -1,13 +1,13 @@
#pragma once
#include <gflags/gflags.h>
#include <esim/rendering/renderer_base.hpp>
#include <esim/rendering/simple_renderer_base.hpp>
#include <gflags/gflags.h>
namespace event_camera_simulator {
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat *img);
Renderer::Ptr loadRendererFromGflags();
SimpleRenderer::Ptr loadSimpleRendererFromGflags();
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img);
Renderer::Ptr loadRendererFromGflags();
SimpleRenderer::Ptr loadSimpleRendererFromGflags();
} // namespace event_camera_simulator

27
event_camera_simulator/esim_data_provider/src/data_provider_base.cpp
View File

@ -2,32 +2,25 @@
namespace event_camera_simulator {
DataProviderBase::DataProviderBase(DataProviderType type)
: type_(type)
, signal_handler_(running_)
{}
DataProviderBase::DataProviderBase(DataProviderType type)
: type_(type),
signal_handler_(running_) {}
void DataProviderBase::spin()
{
void DataProviderBase::spin() {
while (ok())
{
spinOnce();
}
}
void DataProviderBase::pause()
{
void DataProviderBase::pause() {
running_ = false;
}
}
void DataProviderBase::shutdown()
{
void DataProviderBase::shutdown() {
running_ = false;
}
}
void DataProviderBase::registerCallback(const Callback& callback)
{
void DataProviderBase::registerCallback(const Callback& callback) {
callback_ = callback;
}
}
} // namespace event_camera_simulator

93
event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp
View File

@ -1,38 +1,50 @@
#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_factory.hpp>
#include <esim/data_provider/data_provider_from_folder.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 <ze/common/logging.hpp>
DEFINE_int32(data_source, 0, " 0: Online renderer");
DEFINE_double(simulation_minimum_framerate, 30.0,
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");
"that frames are still published at a minimum framerate"
);
DEFINE_double(simulation_imu_rate, 1000.0,
"Fixed IMU sampling frequency, in Hz");
DEFINE_double(
simulation_imu_rate, 1000.0, "Fixed IMU sampling frequency, in Hz"
);
DEFINE_int32(simulation_adaptive_sampling_method, 0,
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");
"1: based on optic flow"
);
DEFINE_double(simulation_adaptive_sampling_lambda, 0.5,
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.");
"...based on predicted absolute brightness change: deltaT = lambda / "
"max(|dL/dt|)"
"...based on optic flow: deltaT = lambda \ max(||du/dt||) where du/dt "
"denotes the 2D optic flow field."
);
DEFINE_string(path_to_data_folder, "", "Path to folder containing the data.");
// Parameters for the DataProviderRosbag
DEFINE_string(bag_filename, "dataset.bag", "Name of bagfile from which to read.");
DEFINE_string(
bag_filename, "dataset.bag", "Name of bagfile from which to read."
);
DEFINE_string(topic_cam0, "/cam0/image_raw", "");
DEFINE_string(topic_cam1, "/cam1/image_raw", "");
DEFINE_string(topic_cam2, "/cam2/image_raw", "");
@ -41,59 +53,64 @@ DEFINE_uint64(num_cams, 1, "Number of normal cameras to read from rosbag.");
namespace event_camera_simulator {
DataProviderBase::Ptr loadDataProviderFromGflags()
{
DataProviderBase::Ptr loadDataProviderFromGflags() {
// Create data provider.
DataProviderBase::Ptr data_provider;
switch (FLAGS_data_source)
{
switch (FLAGS_data_source) {
case 0: // Online Renderer for Moving 3D Camera Rig with IMU
{
data_provider.reset(
new DataProviderOnlineMoving3DCameraRig(FLAGS_simulation_minimum_framerate,
data_provider.reset(new DataProviderOnlineMoving3DCameraRig(
FLAGS_simulation_minimum_framerate,
FLAGS_simulation_imu_rate,
FLAGS_simulation_adaptive_sampling_method,
FLAGS_simulation_adaptive_sampling_lambda));
FLAGS_simulation_adaptive_sampling_lambda
));
break;
}
case 1: // Online Renderer Simple
{
data_provider.reset(
new DataProviderOnlineSimple(FLAGS_simulation_minimum_framerate,
data_provider.reset(new DataProviderOnlineSimple(
FLAGS_simulation_minimum_framerate,
FLAGS_simulation_adaptive_sampling_method,
FLAGS_simulation_adaptive_sampling_lambda));
FLAGS_simulation_adaptive_sampling_lambda
));
break;
}
case 2: // Read data from a folder
{
data_provider.reset(
new DataProviderFromFolder(FLAGS_path_to_data_folder));
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";
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;
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));
new DataProviderRosbag(FLAGS_bag_filename, cam_topics)
);
break;
}
default:
{
default: {
LOG(FATAL) << "Data source not known.";
break;
}
}
return data_provider;
}
}
} // namespace ze
} // namespace event_camera_simulator

79
event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp
View File

@ -1,31 +1,45 @@
#include <esim/data_provider/data_provider_from_folder.hpp>
#include <ze/common/file_utils.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <ze/cameras/camera_impl.hpp>
#include <ze/common/file_utils.hpp>
namespace event_camera_simulator {
DataProviderFromFolder::DataProviderFromFolder(const std::string& path_to_data_folder)
DataProviderFromFolder::DataProviderFromFolder(
const std::string& path_to_data_folder
)
: DataProviderBase(DataProviderType::Folder),
path_to_data_folder_(path_to_data_folder),
finished_parsing_(false)
{
finished_parsing_(false) {
// Load CSV image file
images_in_str_.open(ze::joinPath(path_to_data_folder, "images.csv"));
CHECK(images_in_str_.is_open());
// Create dummy camera rig
// these intrinsic values are filled with garbage (width = height = 1) since the actual values are not known
// these intrinsic values are filled with garbage (width = height = 1)
// since the actual values are not known
ze::CameraVector cameras;
cameras.emplace_back(ze::createEquidistantCameraShared(1, 1, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
cameras.emplace_back(ze::createEquidistantCameraShared(
1,
1,
1.0,
1.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
));
ze::TransformationVector extrinsics;
extrinsics.push_back(ze::Transformation());
camera_rig_ = std::make_shared<ze::CameraRig>(extrinsics, cameras, "camera");
camera_rig_ =
std::make_shared<ze::CameraRig>(extrinsics, cameras, "camera");
// Allocate memory for image data
sim_data_.images.emplace_back(ImagePtr(new Image(
cv::Size(camera_rig_->at(0).width(),
camera_rig_->at(0).height()))));
cv::Size(camera_rig_->at(0).width(), camera_rig_->at(0).height())
)));
sim_data_.camera_rig = camera_rig_;
sim_data_.images_updated = true;
@ -34,59 +48,58 @@ DataProviderFromFolder::DataProviderFromFolder(const std::string& path_to_data_f
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
{
int64_t DataProviderFromFolder::getTimeStamp(const std::string& ts_str
) const {
return std::stoll(ts_str);
}
}
size_t DataProviderFromFolder::numCameras() const
{
size_t DataProviderFromFolder::numCameras() const {
return camera_rig_->size();
}
}
bool DataProviderFromFolder::spinOnce()
{
bool DataProviderFromFolder::spinOnce() {
std::string line;
if(!getline(images_in_str_, line))
{
if (!getline(images_in_str_, line)) {
VLOG(1) << "Finished parsing images.csv file";
finished_parsing_ = true;
return false;
}
if('%' != line.at(0) && '#' != line.at(0))
{
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_, items[1]);
const std::string& path_to_image =
ze::joinPath(path_to_data_folder_, items[1]);
cv::Mat image = cv::imread(path_to_image, 0);
CHECK(image.data) << "Could not load image from file: " << path_to_image;
CHECK(image.data)
<< "Could not load image from file: " << path_to_image;
VLOG(3) << "Read image from file: " << path_to_image;
image.convertTo(*sim_data_.images[0], cv::DataType<ImageFloatType>::type, 1./255.);
image.convertTo(
*sim_data_.images[0],
cv::DataType<ImageFloatType>::type,
1. / 255.
);
if(callback_)
{
if (callback_) {
sim_data_.timestamp = static_cast<Time>(stamp);
callback_(sim_data_);
}
}
return true;
}
}
bool DataProviderFromFolder::ok() const
{
if (!running_ || finished_parsing_)
{
bool DataProviderFromFolder::ok() const {
if (!running_ || finished_parsing_) {
VLOG(1) << "Data Provider was paused/terminated.";
return false;
}
return true;
}
}
} // namespace event_camera_simulator

382
event_camera_simulator/esim_data_provider/src/data_provider_online_render.cpp
View File

@ -1,42 +1,54 @@
#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 <esim/data_provider/data_provider_online_render.hpp>
#include <esim/data_provider/renderer_factory.hpp>
#include <esim/trajectory/imu_factory.hpp>
#include <esim/trajectory/trajectory_factory.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_,
DECLARE_TIMER(
TimerDataProvider,
timers_data_provider_,
render,
optic_flow,
sample_frame,
sample_imu
);
);
DEFINE_double(simulation_post_gaussian_blur_sigma, 0,
DEFINE_double(
simulation_post_gaussian_blur_sigma,
0,
"If sigma > 0, Gaussian blur the renderered images"
"with a Gaussian filter standard deviation sigma.");
"with a Gaussian filter standard deviation sigma."
);
namespace event_camera_simulator {
DataProviderOnlineMoving3DCameraRig::DataProviderOnlineMoving3DCameraRig(ze::real_t simulation_minimum_framerate,
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)
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);
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();
std::tie(trajectory_, trajectory_dyn_obj_) =
loadTrajectorySimulatorFromGflags();
imu_ = loadImuSimulatorFromGflags(trajectory_);
camera_rig_ = ze::cameraRigFromGflags();
@ -46,30 +58,37 @@ DataProviderOnlineMoving3DCameraRig::DataProviderOnlineMoving3DCameraRig(ze::rea
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))));
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))));
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))));
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)
{
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)));
optic_flow_helpers_.emplace_back(
std::make_shared<OpticFlowHelper>(camera_rig_->atShared(i))
);
}
const size_t num_cameras = camera_rig_->size();
@ -77,25 +96,29 @@ DataProviderOnlineMoving3DCameraRig::DataProviderOnlineMoving3DCameraRig(ze::rea
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());
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_.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++)
{
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_.groundtruth.linear_velocity_obj_.push_back(LinearVelocity(
));
sim_data_.groundtruth.angular_velocity_obj_.push_back(
AngularVelocity()
);
}
sim_data_.camera_rig = camera_rig_;
@ -105,109 +128,113 @@ DataProviderOnlineMoving3DCameraRig::DataProviderOnlineMoving3DCameraRig(ze::rea
sampleImu();
sampleFrame();
setAllUpdated();
if(callback_)
{
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();
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()
{
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)
{
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 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);
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_));
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()
{
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();
auto t =
timers_data_provider_[::TimerDataProvider::sample_imu].timeScope();
if(t_ > trajectory_->end())
{
if (t_ > trajectory_->end())
return;
}
updateGroundtruth();
sim_data_.specific_force_corrupted = imu_->specificForceCorrupted(t_);
sim_data_.angular_velocity_corrupted = imu_->angularVelocityCorrupted(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()
{
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();
// 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())
{
if (t_ > trajectory_->end())
return;
}
updateGroundtruth();
{
auto t = timers_data_provider_[::TimerDataProvider::render].timeScope();
auto t =
timers_data_provider_[::TimerDataProvider::render].timeScope();
for(size_t i=0; i<camera_rig_->size(); ++i)
{
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),
// 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_,
@ -215,59 +242,63 @@ void DataProviderOnlineMoving3DCameraRig::sampleFrame()
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_.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]);
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);
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)
{
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],
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]);
sim_data_.depthmaps[i],
sim_data_.optic_flows[i]
);
}
}
}
// Adaptive sampling scheme based on predicted brightness change
if(simulation_adaptive_sampling_method_ == 0)
{
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]));
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());
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;
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;
@ -276,50 +307,55 @@ void DataProviderOnlineMoving3DCameraRig::sampleFrame()
// 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]));
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());
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";
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;
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
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
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()
{
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;
@ -327,27 +363,33 @@ void DataProviderOnlineMoving3DCameraRig::setAllUpdated()
sim_data_.images_updated = true;
sim_data_.depthmaps_updated = true;
sim_data_.optic_flows_updated = true;
}
}
bool DataProviderOnlineMoving3DCameraRig::spinOnce()
{
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:
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.
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 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.
* 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).
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_;
@ -358,29 +400,22 @@ bool DataProviderOnlineMoving3DCameraRig::spinOnce()
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_)
{
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_frame)
{
} else if (next_t_flow_ < next_t_imu && next_t_flow_ < next_t_frame) {
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_)
{
} 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
{
} else {
VLOG(2) << "Sample IMU and frame";
t_ = next_t_frame;
// In that case, we sample everything
@ -389,32 +424,27 @@ bool DataProviderOnlineMoving3DCameraRig::spinOnce()
setAllUpdated();
}
if(t_ > trajectory_->end())
{
if (t_ > trajectory_->end()) {
running_ = false;
return false;
}
if(callback_)
{
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.";
} else {
LOG_FIRST_N(WARNING, 1)
<< "No camera callback registered but measurements available.";
}
return true;
}
}
bool DataProviderOnlineMoving3DCameraRig::ok() const
{
if (!running_)
{
bool DataProviderOnlineMoving3DCameraRig::ok() const {
if (!running_) {
VLOG(1) << "Data Provider was paused/terminated.";
return false;
}
return true;
}
}
} // namespace event_camera_simulator

154
event_camera_simulator/esim_data_provider/src/data_provider_online_simple.cpp
View File

@ -1,35 +1,42 @@
#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>
#include <esim/data_provider/data_provider_online_simple.hpp>
#include <esim/data_provider/renderer_factory.hpp>
#include <ze/common/time_conversions.hpp>
DECLARE_double(simulation_post_gaussian_blur_sigma);
namespace event_camera_simulator {
DataProviderOnlineSimple::DataProviderOnlineSimple(ze::real_t simulation_minimum_framerate,
DataProviderOnlineSimple::DataProviderOnlineSimple(
ze::real_t simulation_minimum_framerate,
int simulation_adaptive_sampling_method,
ze::real_t simulation_adaptive_sampling_lambda)
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);
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());
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_.optic_flows.emplace_back(OpticFlowPtr(new OpticFlow(size))
);
sim_data_.images[i]->setTo(0);
sim_data_.optic_flows[i]->setTo(0);
@ -39,56 +46,55 @@ DataProviderOnlineSimple::DataProviderOnlineSimple(ze::real_t simulation_minimum
t_ = 0.;
sampleFrame();
setFrameUpdated();
if(callback_)
{
if (callback_)
callback_(sim_data_);
}
}
DataProviderOnlineSimple::~DataProviderOnlineSimple()
{
}
DataProviderOnlineSimple::~DataProviderOnlineSimple() {}
size_t DataProviderOnlineSimple::numCameras() const
{
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;
}
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);
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)
{
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]);
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;
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;
@ -96,14 +102,17 @@ bool DataProviderOnlineSimple::sampleFrame()
// Adaptive sampling scheme based on optic flow
else {
const FloatType max_flow_magnitude = maxMagnitudeOpticFlow(sim_data_.optic_flows[0]);
const FloatType max_flow_magnitude =
maxMagnitudeOpticFlow(sim_data_.optic_flows[0]);
VLOG(1) << "max(||optic_flow||) = " << max_flow_magnitude << " px/s";
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;
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;
@ -112,66 +121,57 @@ bool DataProviderOnlineSimple::sampleFrame()
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
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()
{
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)
{
if (next_t_adaptive_ < next_t_frame) {
VLOG(2) << "Sample frame (because of optic flow)";
t_ = next_t_adaptive_;
}
else
{
} else {
VLOG(2) << "Sample frame (because of minimum framerate)";
t_ = next_t_frame;
}
bool is_finished = sampleFrame();
setFrameUpdated();
if(is_finished)
{
if (is_finished) {
running_ = false;
return false;
}
if(callback_)
{
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.";
} else {
LOG_FIRST_N(WARNING, 1)
<< "No camera callback registered but measurements available.";
}
return true;
}
}
bool DataProviderOnlineSimple::ok() const
{
if (!running_)
{
bool DataProviderOnlineSimple::ok() const {
if (!running_) {
VLOG(1) << "Data Provider was paused/terminated.";
return false;
}
return true;
}
}
} // namespace event_camera_simulator

232
event_camera_simulator/esim_data_provider/src/data_provider_rosbag.cpp
View File

@ -1,110 +1,106 @@
#include <esim/data_provider/data_provider_rosbag.hpp>
#include <ze/common/logging.hpp>
#include <rosbag/query.h>
#include <cv_bridge/cv_bridge.h>
#include <esim/data_provider/data_provider_rosbag.hpp>
#include <rosbag/query.h>
#include <sensor_msgs/image_encodings.h>
#include <ze/common/time_conversions.hpp>
#include <ze/common/string_utils.hpp>
#include <ze/common/logging.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/string_utils.hpp>
#include <ze/common/time_conversions.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");
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(
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)
{
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_)
{
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;
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
{
try {
bag_->open(bag_filename, rosbag::bagmode::Read);
} catch (const std::exception e) {
LOG(FATAL) << "Could not open rosbag " << bag_filename << ": "
<< e.what();
}
catch (const std::exception e)
{
LOG(FATAL) << "Could not open rosbag " << bag_filename << ": " << e.what();
}
}
void DataProviderRosbag::initBagView(const std::vector<std::string>& topics)
{
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)
{
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).
// 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
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);
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);
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;
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. "
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. "
} 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
{
} else {
VLOG(1) << "Absolute start time set to " << absolute_start_time;
VLOG(1) << "Absolute stop time set to " << absolute_stop_time;
}
@ -112,94 +108,97 @@ void DataProviderRosbag::initBagView(const std::vector<std::string>& topics)
// 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_.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.
// 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:";
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. "
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
{
size_t DataProviderRosbag::numCameras() const {
return img_topic_camidx_map_.size();
}
}
bool DataProviderRosbag::spinOnce()
{
if (bag_view_it_ != bag_view_->end())
{
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_)
{
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";
} 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)
{
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())
{
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.";
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)
{
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());
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;
@ -209,34 +208,29 @@ bool DataProviderRosbag::cameraSpin(sensor_msgs::ImageConstPtr m_img,
sim_data_.poses_updated = false;
callback_(sim_data_);
}
else
{
LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
} else {
LOG_FIRST_N(WARNING, 1)
<< "Topic in bag that is not subscribed: " << m.getTopic();
}
return true;
}
}
bool DataProviderRosbag::ok() const
{
if (!running_)
{
bool DataProviderRosbag::ok() const {
if (!running_) {
VLOG(1) << "Data Provider was paused/terminated.";
return false;
}
if (bag_view_it_ == bag_view_->end())
{
if (bag_view_it_ == bag_view_->end()) {
VLOG(1) << "All data processed.";
return false;
}
return true;
}
}
size_t DataProviderRosbag::size() const
{
size_t DataProviderRosbag::size() const {
CHECK(bag_view_);
return bag_view_->size();
}
}
} // namespace event_camera_simulator

252
event_camera_simulator/esim_data_provider/src/renderer_factory.cpp
View File

@ -1,155 +1,231 @@
#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 <esim/imp_opengl_renderer/opengl_renderer.hpp>
#include <esim/imp_panorama_renderer/panorama_renderer.hpp>
#include <esim/imp_planar_renderer/planar_renderer.hpp>
#include <esim/imp_unrealcv_renderer/unrealcv_renderer.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/cameras/camera_impl.hpp>
#include <ze/cameras/camera_models.hpp>
#include <ze/common/logging.hpp>
DEFINE_int32(renderer_type, 0, " 0: Planar renderer, 1: Panorama renderer, 2: OpenGL renderer");
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_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_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_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_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_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_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_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_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_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_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_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_min, 0.01, "Minimum clipping distance.");
DEFINE_double(renderer_z_max, 10.0,
"Maximum 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_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_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_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_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)");
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)
{
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img) {
CHECK(img);
VLOG(1) << "Loading texture file from file: " << FLAGS_renderer_texture << ".";
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 << ".";
if (!img->data) {
LOG(FATAL) << "Could not open image at: " << FLAGS_renderer_texture
<< ".";
return false;
}
if(FLAGS_renderer_preprocess_median_blur > 1)
{
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: "
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);
cv::GaussianBlur(
*img,
*img,
cv::Size(21, 21),
FLAGS_renderer_preprocess_gaussian_blur
);
}
img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0/255.0);
img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0 / 255.0);
return true;
}
}
Renderer::Ptr loadRendererFromGflags()
{
Renderer::Ptr loadRendererFromGflags() {
Renderer::Ptr renderer;
switch (FLAGS_renderer_type)
{
switch (FLAGS_renderer_type) {
case 0: // Planar renderer
{
cv::Mat img_src;
if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
{
if (!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
return nullptr;
}
double f_src = hfovToFocalLength(FLAGS_renderer_hfov_cam_source_deg, img_src.cols);
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());
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,
T_W_P.getPosition() = ze::Position(
FLAGS_renderer_plane_x,
FLAGS_renderer_plane_y,
FLAGS_renderer_plane_z);
FLAGS_renderer_plane_z
);
T_W_P.getRotation() = ze::Quaternion(FLAGS_renderer_plane_qw,
T_W_P.getRotation() = ze::Quaternion(
FLAGS_renderer_plane_qw,
FLAGS_renderer_plane_qx,
FLAGS_renderer_plane_qy,
FLAGS_renderer_plane_qz);
FLAGS_renderer_plane_qz
);
renderer.reset(new PlanarRenderer(img_src, cam_src,
renderer.reset(new PlanarRenderer(
img_src,
cam_src,
T_W_P,
FLAGS_renderer_z_min,
FLAGS_renderer_z_max,
FLAGS_renderer_extend_border));
FLAGS_renderer_extend_border
));
break;
}
case 1: // Panorama renderer
{
cv::Mat img_src;
if(!loadPreprocessedImage(FLAGS_renderer_texture, &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,
R_W_P = ze::Quaternion(
FLAGS_renderer_panorama_qw,
FLAGS_renderer_panorama_qx,
FLAGS_renderer_panorama_qy,
FLAGS_renderer_panorama_qz);
FLAGS_renderer_panorama_qz
);
renderer.reset(new PanoramaRenderer(img_src, R_W_P));
break;
@ -164,36 +240,32 @@ Renderer::Ptr loadRendererFromGflags()
renderer.reset(new UnrealCvRenderer());
break;
}
default:
{
default: {
LOG(FATAL) << "Renderer type is not known.";
break;
}
}
return renderer;
}
}
SimpleRenderer::Ptr loadSimpleRendererFromGflags()
{
SimpleRenderer::Ptr loadSimpleRendererFromGflags() {
SimpleRenderer::Ptr renderer;
switch (FLAGS_renderer_type)
{
switch (FLAGS_renderer_type) {
case 0: // Multi-objects 2D renderer
{
renderer.reset(new MultiObject2DRenderer());
break;
}
default:
{
default: {
LOG(FATAL) << "Renderer type is not known.";
break;
}
}
return renderer;
}
}
} // namespace event_camera_simulator

36
event_camera_simulator/esim_rendering/include/esim/rendering/renderer_base.hpp
View File

@ -1,32 +1,35 @@
#pragma once
#include <ze/common/macros.hpp>
#include <esim/common/types.hpp>
#include <ze/common/macros.hpp>
namespace event_camera_simulator {
//! Represents a rendering engine that generates images (and other outputs,
//! such as depth maps, or optical flow maps) given a scene and a camera position.
class Renderer
{
public:
//! Represents a rendering engine that generates images (and other outputs,
//! such as depth maps, or optical flow maps) given a scene and a camera
//! position.
class Renderer {
public:
ZE_POINTER_TYPEDEFS(Renderer);
Renderer() {}
//! Render an image at a given pose.
virtual void render(const Transformation& T_W_C,
virtual void render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap) const = 0;
const DepthmapPtr& out_depthmap
) const = 0;
//! Returns true if the rendering engine can compute optic flow, false otherwise
//! Returns true if the rendering engine can compute optic flow, false
//! otherwise
virtual bool canComputeOpticFlow() const = 0;
//! Render an image + depth map + optic flow map at a given pose,
//! given the camera linear and angular velocity
virtual void renderWithFlow(const Transformation& T_W_C,
virtual void renderWithFlow(
const Transformation& T_W_C,
const LinearVelocity& v_WC,
const AngularVelocity& w_WC,
const std::vector<Transformation>& T_W_OBJ,
@ -34,16 +37,19 @@ public:
const std::vector<AngularVelocity>& angular_velocity_obj,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap,
const OpticFlowPtr& optic_flow_map) const {}
const OpticFlowPtr& optic_flow_map
) const {}
//! Sets the camera
virtual void setCamera(const ze::Camera::Ptr& camera) = 0;
//! Get the camera rig
const ze::Camera::Ptr& getCamera() const { return camera_; }
const ze::Camera::Ptr& getCamera() const {
return camera_;
}
protected:
protected:
ze::Camera::Ptr camera_;
};
};
} // namespace event_camera_simulator

25
event_camera_simulator/esim_rendering/include/esim/rendering/simple_renderer_base.hpp
View File

@ -1,24 +1,27 @@
#pragma once
#include <ze/common/macros.hpp>
#include <esim/common/types.hpp>
#include <ze/common/macros.hpp>
namespace event_camera_simulator {
//! Represents a rendering engine that generates images + optic flow maps
//! The rendering engine takes care of managing the environment and camera trajectory in the environment
class SimpleRenderer
{
public:
//! Represents a rendering engine that generates images + optic flow maps
//! The rendering engine takes care of managing the environment and camera
//! trajectory in the environment
class SimpleRenderer {
public:
ZE_POINTER_TYPEDEFS(SimpleRenderer);
SimpleRenderer() {}
//! Render an image + optic flow map at a given time t.
//! The rendering engine takes care of generating the camera trajectory, etc.
virtual bool render(const Time t,
//! The rendering engine takes care of generating the camera trajectory,
//! etc.
virtual bool render(
const Time t,
const ImagePtr& out_image,
const OpticFlowPtr& optic_flow_map) const = 0;
const OpticFlowPtr& optic_flow_map
) const = 0;
//! Get the height of the image plane
virtual int getWidth() const = 0;
@ -26,7 +29,7 @@ public:
//! Get the width of the image plane
virtual int getHeight() const = 0;
protected:
};
protected:
};
} // namespace event_camera_simulator

4
event_camera_simulator/esim_rendering/src/renderer_base.cpp
View File

@ -1,5 +1,3 @@
#include <esim/rendering/renderer_base.hpp>
namespace event_camera_simulator {
} // namespace event_camera_simulator
namespace event_camera_simulator {} // namespace event_camera_simulator

4
event_camera_simulator/esim_rendering/src/simple_renderer_base.cpp
View File

@ -1,5 +1,3 @@
#include <esim/rendering/simple_renderer_base.hpp>
namespace event_camera_simulator {
} // namespace event_camera_simulator
namespace event_camera_simulator {} // namespace event_camera_simulator

5
event_camera_simulator/esim_trajectory/include/esim/trajectory/imu_factory.hpp
View File

@ -1,11 +1,12 @@
#pragma once
#include <gflags/gflags.h>
#include <ze/vi_simulation/trajectory_simulator.hpp>
#include <ze/vi_simulation/imu_simulator.hpp>
#include <ze/vi_simulation/trajectory_simulator.hpp>
namespace event_camera_simulator {
ze::ImuSimulator::Ptr loadImuSimulatorFromGflags(const ze::TrajectorySimulator::Ptr &trajectory);
ze::ImuSimulator::Ptr
loadImuSimulatorFromGflags(const ze::TrajectorySimulator::Ptr& trajectory);
} // namespace event_camera_simulator

5
event_camera_simulator/esim_trajectory/include/esim/trajectory/trajectory_factory.hpp
View File

@ -5,6 +5,9 @@
namespace event_camera_simulator {
std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Ptr>> loadTrajectorySimulatorFromGflags();
std::tuple<
ze::TrajectorySimulator::Ptr,
std::vector<ze::TrajectorySimulator::Ptr>>
loadTrajectorySimulatorFromGflags();
} // namespace event_camera_simulator

34
event_camera_simulator/esim_trajectory/src/imu_factory.cpp
View File

@ -1,10 +1,10 @@
#include <ze/common/logging.hpp>
#include <esim/trajectory/imu_factory.hpp>
#include <ze/common/logging.hpp>
namespace event_camera_simulator {
ze::ImuSimulator::Ptr loadImuSimulatorFromGflags(const ze::TrajectorySimulator::Ptr& trajectory)
{
ze::ImuSimulator::Ptr
loadImuSimulatorFromGflags(const ze::TrajectorySimulator::Ptr& trajectory) {
ze::ImuSimulator::Ptr imu;
const ze::real_t gyr_bias_noise_sigma = 0.0000266;
@ -15,20 +15,20 @@ ze::ImuSimulator::Ptr loadImuSimulatorFromGflags(const ze::TrajectorySimulator::
const ze::real_t gravity_magnitude = 9.81;
ze::ImuBiasSimulator::Ptr bias;
try
{
try {
VLOG(1) << "Initialize bias ...";
bias = std::make_shared<ze::ContinuousBiasSimulator>(
ze::Vector3::Constant(gyr_bias_noise_sigma),
ze::Vector3::Constant(acc_bias_noise_sigma),
trajectory->start(),
trajectory->end(),
100); // Results in malloc: (trajectory->end() - trajectory->start()) * imu_bandwidth_hz);
100
); // Results in malloc: (trajectory->end() - trajectory->start()) *
// imu_bandwidth_hz);
VLOG(1) << "done.";
}
catch (const std::bad_alloc& e)
{
LOG(FATAL) << "Could not create bias because number of samples is too high."
} catch (const std::bad_alloc& e) {
LOG(FATAL
) << "Could not create bias because number of samples is too high."
<< " Allocation failed: " << e.what();
}
@ -36,15 +36,19 @@ ze::ImuSimulator::Ptr loadImuSimulatorFromGflags(const ze::TrajectorySimulator::
imu = std::make_shared<ze::ImuSimulator>(
trajectory,
bias,
ze::RandomVectorSampler<3>::sigmas(ze::Vector3::Constant(acc_noise_sigma)),
ze::RandomVectorSampler<3>::sigmas(ze::Vector3::Constant(gyr_noise_sigma)),
ze::RandomVectorSampler<3>::sigmas(
ze::Vector3::Constant(acc_noise_sigma)
),
ze::RandomVectorSampler<3>::sigmas(
ze::Vector3::Constant(gyr_noise_sigma)
),
imu_bandwidth_hz,
imu_bandwidth_hz,
gravity_magnitude);
gravity_magnitude
);
VLOG(1) << "done.";
return imu;
}
}
} // namespace event_camera_simulator

255
event_camera_simulator/esim_trajectory/src/trajectory_factory.cpp
View File

@ -2,87 +2,120 @@
#include <ze/common/csv_trajectory.hpp>
#include <ze/common/logging.hpp>
DEFINE_int32(trajectory_type, 0, " 0: Random spline trajectory, 1: Load trajectory from CSV file");
DEFINE_int32(
trajectory_type,
0,
" 0: Random spline trajectory, 1: Load trajectory from CSV file"
);
DEFINE_double(trajectory_length_s, 100.0,
DEFINE_double(
trajectory_length_s,
100.0,
"Length of the trajectory, in seconds"
"(used when the trajectory type is random spline)");
"(used when the trajectory type is random spline)"
);
DEFINE_int32(trajectory_spline_order, 5,
"Spline order of the trajectory");
DEFINE_int32(trajectory_spline_order, 5, "Spline order of the trajectory");
DEFINE_double(trajectory_sampling_frequency_hz, 5,
DEFINE_double(
trajectory_sampling_frequency_hz,
5,
"Sampling frequency of the spline trajectory, i.e."
"number of random poses generated per second along the trajectory");
"number of random poses generated per second along the trajectory"
);
DEFINE_int32(trajectory_num_spline_segments, 100,
"Number of spline segments used for the trajectory");
DEFINE_int32(
trajectory_num_spline_segments,
100,
"Number of spline segments used for the trajectory"
);
DEFINE_double(trajectory_lambda, 0.1,
DEFINE_double(
trajectory_lambda,
0.1,
"Smoothing factor for the spline trajectories."
"Low value = less smooth, high value = more smooth");
"Low value = less smooth, high value = more smooth"
);
DEFINE_double(trajectory_multiplier_x, 1.0,
"Scaling factor for the X camera axis");
DEFINE_double(
trajectory_multiplier_x, 1.0, "Scaling factor for the X camera axis"
);
DEFINE_double(trajectory_multiplier_y, 1.0,
"Scaling factor for the y camera axis");
DEFINE_double(
trajectory_multiplier_y, 1.0, "Scaling factor for the y camera axis"
);
DEFINE_double(trajectory_multiplier_z, 1.0,
"Scaling factor for the z camera axis");
DEFINE_double(
trajectory_multiplier_z, 1.0, "Scaling factor for the z camera axis"
);
DEFINE_double(trajectory_multiplier_wx, 1.0,
"Scaling factor for the x orientation axis");
DEFINE_double(
trajectory_multiplier_wx, 1.0, "Scaling factor for the x orientation axis"
);
DEFINE_double(trajectory_multiplier_wy, 1.0,
"Scaling factor for the y orientation axis");
DEFINE_double(
trajectory_multiplier_wy, 1.0, "Scaling factor for the y orientation axis"
);
DEFINE_double(trajectory_multiplier_wz, 1.0,
"Scaling factor for the z orientation axis");
DEFINE_double(
trajectory_multiplier_wz, 1.0, "Scaling factor for the z orientation axis"
);
DEFINE_double(trajectory_offset_x, 0.0,
"Offset for the X camera axis");
DEFINE_double(trajectory_offset_x, 0.0, "Offset for the X camera axis");
DEFINE_double(trajectory_offset_y, 0.0,
"Offset for the y camera axis");
DEFINE_double(trajectory_offset_y, 0.0, "Offset for the y camera axis");
DEFINE_double(trajectory_offset_z, 0.0,
"Offset for the z camera axis");
DEFINE_double(trajectory_offset_z, 0.0, "Offset for the z camera axis");
DEFINE_double(trajectory_offset_wx, 0.0,
"Offset for the x orientation axis");
DEFINE_double(trajectory_offset_wx, 0.0, "Offset for the x orientation axis");
DEFINE_double(trajectory_offset_wy, 0.0,
"Offset for the y orientation axis");
DEFINE_double(trajectory_offset_wy, 0.0, "Offset for the y orientation axis");
DEFINE_double(trajectory_offset_wz, 0.0,
"Offset for the z orientation axis");
DEFINE_double(trajectory_offset_wz, 0.0, "Offset for the z orientation axis");
DEFINE_string(trajectory_csv_file, "",
"CSV file containing the trajectory");
DEFINE_string(trajectory_csv_file, "", "CSV file containing the trajectory");
DEFINE_int32(trajectory_dynamic_objects_type, 1, " 0: Random spline trajectory, 1: Load trajectory from CSV file");
DEFINE_int32(
trajectory_dynamic_objects_type,
1,
" 0: Random spline trajectory, 1: Load trajectory from CSV file"
);
DEFINE_int32(trajectory_dynamic_objects_spline_order, 5,
"Spline order of the trajectory");
DEFINE_int32(
trajectory_dynamic_objects_spline_order, 5, "Spline order of the trajectory"
);
DEFINE_int32(trajectory_dynamic_objects_num_spline_segments, 100,
"Number of spline segments used for the trajectory");
DEFINE_int32(
trajectory_dynamic_objects_num_spline_segments,
100,
"Number of spline segments used for the trajectory"
);
DEFINE_double(trajectory_dynamic_objects_lambda, 0.1,
DEFINE_double(
trajectory_dynamic_objects_lambda,
0.1,
"Smoothing factor for the spline trajectories."
"Low value = less smooth, high value = more smooth");
"Low value = less smooth, high value = more smooth"
);
DEFINE_string(trajectory_dynamic_objects_csv_dir, "",
"Directory containing CSV file of trajectory for dynamic objects");
DEFINE_string(
trajectory_dynamic_objects_csv_dir,
"",
"Directory containing CSV file of trajectory for dynamic objects"
);
DEFINE_string(trajectory_dynamic_objects_csv_file, "",
"CSV file containing the trajectory");
DEFINE_string(
trajectory_dynamic_objects_csv_file,
"",
"CSV file containing the trajectory"
);
namespace event_camera_simulator {
std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Ptr>> loadTrajectorySimulatorFromGflags()
{
std::tuple<
ze::TrajectorySimulator::Ptr,
std::vector<ze::TrajectorySimulator::Ptr>>
loadTrajectorySimulatorFromGflags() {
ze::TrajectorySimulator::Ptr trajectory;
std::vector<ze::TrajectorySimulator::Ptr> trajectories_dynamic_objects;
@ -90,19 +123,15 @@ std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Pt
size_t p_start, p_end;
p_start = 0;
while(1)
{
while (1) {
int trajectory_type;
if (dynamic_object)
{
if (dynamic_object) {
trajectory_type = FLAGS_trajectory_dynamic_objects_type;
if (trajectory_type != 1)
{
LOG(FATAL) << "Only supporting trajectories of type 1 for dynamic objects!";
if (trajectory_type != 1) {
LOG(FATAL) << "Only supporting trajectories of type 1 for "
"dynamic objects!";
}
}
else
{
} else {
trajectory_type = FLAGS_trajectory_type;
}
@ -110,35 +139,43 @@ std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Pt
std::string csv_path;
bool should_break = false;
if (dynamic_object)
{
if ((p_end = FLAGS_trajectory_dynamic_objects_csv_file.find(";",p_start)) != std::string::npos)
{
csv_path = FLAGS_trajectory_dynamic_objects_csv_dir + "/" + FLAGS_trajectory_dynamic_objects_csv_file.substr(p_start, p_end - p_start);
if (dynamic_object) {
if ((p_end = FLAGS_trajectory_dynamic_objects_csv_file
.find(";", p_start))
!= std::string::npos) {
csv_path = FLAGS_trajectory_dynamic_objects_csv_dir + "/"
+ FLAGS_trajectory_dynamic_objects_csv_file.substr(
p_start,
p_end - p_start
);
p_start = p_end + 1;
}
else
{
csv_path = FLAGS_trajectory_dynamic_objects_csv_dir + "/" + FLAGS_trajectory_dynamic_objects_csv_file.substr(p_start, p_end - p_start);
} else {
csv_path = FLAGS_trajectory_dynamic_objects_csv_dir + "/"
+ FLAGS_trajectory_dynamic_objects_csv_file.substr(
p_start,
p_end - p_start
);
should_break = true;
}
}
else
{
} else {
csv_path = FLAGS_trajectory_csv_file;
}
switch (trajectory_type)
{
switch (trajectory_type) {
case 0: // Random spline
{
std::shared_ptr<ze::BSplinePoseMinimalRotationVector> pbs =
std::make_shared<ze::BSplinePoseMinimalRotationVector>(FLAGS_trajectory_spline_order);
std::make_shared<ze::BSplinePoseMinimalRotationVector>(
FLAGS_trajectory_spline_order
);
ze::MatrixX poses;
ze::VectorX times = ze::VectorX::LinSpaced(FLAGS_trajectory_sampling_frequency_hz * FLAGS_trajectory_length_s,
ze::VectorX times = ze::VectorX::LinSpaced(
FLAGS_trajectory_sampling_frequency_hz
* FLAGS_trajectory_length_s,
0,
FLAGS_trajectory_length_s);
FLAGS_trajectory_length_s
);
poses.resize(6, times.size());
poses.setRandom();
poses.row(0) *= FLAGS_trajectory_multiplier_x;
@ -153,7 +190,12 @@ std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Pt
poses.row(3).array() += FLAGS_trajectory_offset_wx;
poses.row(4).array() += FLAGS_trajectory_offset_wy;
poses.row(5).array() += FLAGS_trajectory_offset_wz;
pbs->initPoseSpline3(times, poses, FLAGS_trajectory_num_spline_segments, FLAGS_trajectory_lambda);
pbs->initPoseSpline3(
times,
poses,
FLAGS_trajectory_num_spline_segments,
FLAGS_trajectory_lambda
);
trajectory.reset(new ze::SplineTrajectorySimulator(pbs));
break;
}
@ -165,72 +207,67 @@ std::tuple<ze::TrajectorySimulator::Ptr, std::vector<ze::TrajectorySimulator::Pt
LOG(INFO) << "Reading trajectory from CSV file: " << csv_path;
pose_series.load(csv_path);
ze::StampedTransformationVector poses = pose_series.getStampedTransformationVector();
ze::StampedTransformationVector poses =
pose_series.getStampedTransformationVector();
// Set start time of trajectory to zero.
const int64_t offset = poses.at(0).first;
for (ze::StampedTransformation& it : poses)
{
it.first -= offset;
}
LOG(INFO) << "Initializing spline from trajectory (this may take some sime)...";
LOG(INFO) << "Initializing spline from trajectory (this may "
"take some sime)...";
int spline_order, num_spline_segments;
double lambda;
if (dynamic_object)
{
spline_order = FLAGS_trajectory_dynamic_objects_spline_order;
num_spline_segments = FLAGS_trajectory_dynamic_objects_num_spline_segments;
if (dynamic_object) {
spline_order =
FLAGS_trajectory_dynamic_objects_spline_order;
num_spline_segments =
FLAGS_trajectory_dynamic_objects_num_spline_segments;
lambda = FLAGS_trajectory_dynamic_objects_lambda;
}
else
{
} else {
spline_order = FLAGS_trajectory_spline_order;
num_spline_segments = FLAGS_trajectory_num_spline_segments;
lambda = FLAGS_trajectory_lambda;
}
std::shared_ptr<ze::BSplinePoseMinimalRotationVector> bs =
std::make_shared<ze::BSplinePoseMinimalRotationVector>(spline_order);
std::make_shared<ze::BSplinePoseMinimalRotationVector>(
spline_order
);
bs->initPoseSplinePoses(poses, num_spline_segments, lambda);
if (dynamic_object)
{
trajectories_dynamic_objects.push_back(std::make_shared<ze::SplineTrajectorySimulator>(bs));
}
else
{
trajectory = std::make_shared<ze::SplineTrajectorySimulator>(bs);
if (dynamic_object) {
trajectories_dynamic_objects.push_back(
std::make_shared<ze::SplineTrajectorySimulator>(bs)
);
} else {
trajectory =
std::make_shared<ze::SplineTrajectorySimulator>(bs);
}
LOG(INFO) << "Done!";
break;
}
default:
{
default: {
LOG(FATAL) << "Trajectory type is not known.";
break;
}
}
if (!dynamic_object)
{
if (!FLAGS_trajectory_dynamic_objects_csv_dir.empty() && !FLAGS_trajectory_dynamic_objects_csv_file.empty())
{
if (!dynamic_object) {
if (!FLAGS_trajectory_dynamic_objects_csv_dir.empty()
&& !FLAGS_trajectory_dynamic_objects_csv_file.empty()) {
dynamic_object = true;
}
else
{
} else {
break;
}
}
if (should_break)
{
break;
}
}
return std::make_tuple(trajectory, trajectories_dynamic_objects);
}
}
} // namespace event_camera_simulator

52
event_camera_simulator/esim_unrealcv_bridge/include/esim/unrealcv_bridge/unrealcv_bridge.hpp
View File

@ -1,26 +1,22 @@
#pragma once
#include <boost/algorithm/string/predicate.hpp>
#include <boost/asio.hpp>
#include <boost/format.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <string>
#include <chrono>
#include <functional>
#include <iostream>
#include <istream>
#include <ostream>
#include <chrono>
#include <thread>
#include <opencv2/core/core.hpp>
#include <ostream>
#include <string>
#include <thread>
namespace event_camera_simulator {
using boost::asio::ip::tcp;
using boost::asio::ip::tcp;
struct CameraData {
struct CameraData {
uint32_t id;
double_t pitch;
double_t yaw;
@ -28,50 +24,52 @@ struct CameraData {
double_t x;
double_t y;
double_t z;
};
};
class UnrealCvClient {
public:
class UnrealCvClient {
public:
UnrealCvClient(std::string host, std::string port);
~UnrealCvClient();
void saveImage(uint32_t camid, std::string path);
cv::Mat getImage(uint32_t camid);
cv::Mat getDepth(uint32_t camid);
void setCamera(const CameraData & camera);
void setCamera(const CameraData& camera);
void setCameraFOV(float hfov_deg);
void setCameraSize(int width, int height);
protected:
protected:
void sendCommand(std::string command);
template<typename Result>
Result sendCommand(std::string command, std::function<Result(std::istream&, uint32_t)> mapf);
template <typename Result>
Result sendCommand(
std::string command,
std::function<Result(std::istream&, uint32_t)> mapf
);
void send(std::string msg, uint32_t counter);
template<typename Result>
template <typename Result>
Result receive(std::function<Result(std::istream&, uint32_t)> parser);
//must stand before socket_ because of c++ initialization order
// must stand before socket_ because of c++ initialization order
boost::asio::io_service io_service_;
tcp::socket socket_;
mutable uint32_t counter_;
uint32_t delay_;
boost::format angular_format_;
private:
private:
void sleep(uint32_t delay);
void handleError(boost::system::error_code ec);
std::string istreamToString(std::istream& stream, uint32_t size);
void parse_npy_header(unsigned char* buffer,
void parse_npy_header(
unsigned char* buffer,
size_t& word_size,
std::vector<size_t>& shape,
bool& fortran_order);
};
bool& fortran_order
);
};
} // namespace event_camera_simulator

353
event_camera_simulator/esim_unrealcv_bridge/src/unrealcv_bridge.cpp
View File

@ -1,54 +1,54 @@
#include <esim/unrealcv_bridge/unrealcv_bridge.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <glog/logging.h>
#include <cmath>
#include <esim/unrealcv_bridge/unrealcv_bridge.hpp>
#include <glog/logging.h>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <regex>
namespace event_camera_simulator {
using boost::asio::ip::tcp;
using boost::asio::ip::tcp;
// from: https://www.boost.org/doc/libs/1_47_0/doc/html/boost_asio/reference/connect/overload6.html
struct unrealcv_server_connect_condition
{
// from:
// https://www.boost.org/doc/libs/1_47_0/doc/html/boost_asio/reference/connect/overload6.html
struct unrealcv_server_connect_condition {
template <typename Iterator>
Iterator operator()(
const boost::system::error_code& ec,
Iterator next)
{
if(ec)
{
Iterator
operator()(const boost::system::error_code& ec, Iterator next) {
if (ec)
LOG(ERROR) << ec.message();
}
LOG(INFO) << "Trying: " << next->endpoint();
return next;
}
};
};
UnrealCvClient::UnrealCvClient(std::string host, std::string port)
UnrealCvClient::UnrealCvClient(std::string host, std::string port)
: io_service_(),
socket_(io_service_),
counter_(0),
delay_(30){
delay_(30) {
tcp::resolver resolver(io_service_);
tcp::resolver::query query(host, port);
tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
boost::system::error_code ec;
boost::asio::connect(socket_, endpoint_iterator, unrealcv_server_connect_condition(), ec);
if(ec)
{
boost::asio::connect(
socket_,
endpoint_iterator,
unrealcv_server_connect_condition(),
ec
);
if (ec) {
LOG(FATAL) << "Could not connect to UnrealCV server";
return;
}
sleep(500); // long sleep to initiate
// receive the first "we are connected" string
receive<std::string>([this] (std::istream& stream, uint32_t size) -> std::string {
receive<std::string>(
[this](std::istream& stream, uint32_t size) -> std::string {
return this->istreamToString(stream, size);
});
}
);
sleep(delay_);
sendCommand("vrun r.AmbientOcclusionLevels 0");
@ -56,54 +56,63 @@ UnrealCvClient::UnrealCvClient(std::string host, std::string port)
sendCommand("vrun r.DefaultFeature.AntiAliasing 2");
sendCommand("vrun r.DefaultFeature.MotionBlur 0");
sendCommand("vrun r.PostProcessAAQuality 6");
}
}
UnrealCvClient::~UnrealCvClient() {
UnrealCvClient::~UnrealCvClient() {
socket_.close();
}
}
void UnrealCvClient::saveImage(uint32_t camid, std::string path)
{
std::string req = (boost::format("vget /camera/%i/lit %s") % camid % path).str();
void UnrealCvClient::saveImage(uint32_t camid, std::string path) {
std::string req =
(boost::format("vget /camera/%i/lit %s") % camid % path).str();
sendCommand(req);
}
}
cv::Mat UnrealCvClient::getImage(uint32_t camid)
{
std::string req = (boost::format("vget /camera/%i/lit png") % camid).str();
cv::Mat UnrealCvClient::getImage(uint32_t camid) {
std::string req =
(boost::format("vget /camera/%i/lit png") % camid).str();
return sendCommand<cv::Mat>(req, [](std::istream& stream, uint32_t size){
return sendCommand<cv::Mat>(
req,
[](std::istream& stream, uint32_t size) {
std::vector<char> data(size);
stream.read(data.data(), size);
cv::Mat matrixPng = cv::imdecode(cv::Mat(data), 1);
return matrixPng.clone();
});
}
}
);
}
cv::Mat UnrealCvClient::getDepth(uint32_t camid)
{
std::string req = (boost::format("vget /camera/%i/depth npy") % camid).str();
return sendCommand<cv::Mat>(req, [this](std::istream& stream, uint32_t size){
cv::Mat UnrealCvClient::getDepth(uint32_t camid) {
std::string req =
(boost::format("vget /camera/%i/depth npy") % camid).str();
return sendCommand<cv::Mat>(
req,
[this](std::istream& stream, uint32_t size) {
std::vector<char> data(size);
stream.read(data.data(), size);
unsigned char* buffer = (unsigned char *)data.data();
unsigned char* buffer = (unsigned char*) data.data();
/*
* Gather data from the header
*/
std::vector<size_t> img_dims; //if appending, the shape of existing + new data
std::vector<size_t>
img_dims; // if appending, the shape of existing + new data
size_t word_size;
bool fortran_order;
parse_npy_header(buffer, word_size, img_dims, fortran_order);
// https://docs.scipy.org/doc/numpy/neps/npy-format.html
std::string npy_magic(reinterpret_cast<char*>(buffer),6);
uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer+6);
uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer+7);
uint16_t header_str_len = *reinterpret_cast<uint16_t*>(buffer+8);
std::string header(reinterpret_cast<char*>(buffer+9),header_str_len);
std::string npy_magic(reinterpret_cast<char*>(buffer), 6);
uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer + 6);
uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer + 7);
uint16_t header_str_len =
*reinterpret_cast<uint16_t*>(buffer + 8);
std::string header(
reinterpret_cast<char*>(buffer + 9),
header_str_len
);
uint16_t header_total_len = 10 + header_str_len;
uint32_t data_length = data.size() - header_total_len;
@ -112,66 +121,67 @@ cv::Mat UnrealCvClient::getDepth(uint32_t camid)
/*
* Ensure that the data is okay
*/
if(!(major_version == 1 &&
minor_version == 0 &&
npy_magic.find("NUMPY") != std::string::npos)){
if (!(major_version == 1 && minor_version == 0
&& npy_magic.find("NUMPY") != std::string::npos)) {
throw std::runtime_error("Npy header data not supported");
}
if(!(data_length == (num_pixel * sizeof(float_t)))) {
throw std::runtime_error("Npy array data shape does not match the image size");
if (!(data_length == (num_pixel * sizeof(float_t)))) {
throw std::runtime_error(
"Npy array data shape does not match the image size"
);
}
/*
* Read and convert the data
*/
cv::Mat matrix_f32 = cv::Mat(img_dims.at(0), img_dims.at(1),
CV_32F, buffer + header_total_len).clone();
cv::Mat matrix_f32 = cv::Mat(
img_dims.at(0),
img_dims.at(1),
CV_32F,
buffer + header_total_len
)
.clone();
return matrix_f32;
}
);
}
});
}
void UnrealCvClient::setCamera(const CameraData & camera)
{
std::string cam_pose_s = (boost::format("vset /camera/%i/pose %.5f %.5f %.5f %.5f %.5f %.5f") %
camera.id %
camera.x %
camera.y %
camera.z %
camera.pitch %
camera.yaw %
camera.roll).str();
void UnrealCvClient::setCamera(const CameraData& camera) {
std::string cam_pose_s =
(boost::format("vset /camera/%i/pose %.5f %.5f %.5f %.5f %.5f %.5f")
% camera.id % camera.x % camera.y % camera.z % camera.pitch
% camera.yaw % camera.roll)
.str();
sendCommand(cam_pose_s);
}
}
void UnrealCvClient::setCameraSize(int width, int height)
{
void UnrealCvClient::setCameraSize(int width, int height) {
VLOG(1) << "Setting the camera size to: " << width << "x" << height;
std::string req_size = (boost::format("vrun r.setres %dx%d") %
width %
height).str();
std::string req_size =
(boost::format("vrun r.setres %dx%d") % width % height).str();
sendCommand(req_size);
}
}
void UnrealCvClient::setCameraFOV(float hfov_deg)
{
VLOG(1) << "Setting the camera horizontal field of view to: " << hfov_deg << " deg";
void UnrealCvClient::setCameraFOV(float hfov_deg) {
VLOG(1) << "Setting the camera horizontal field of view to: "
<< hfov_deg << " deg";
const int cam_id = 0;
std::string req_fov = (boost::format("vset /camera/%i/horizontal_fieldofview %.5f") %
cam_id %
hfov_deg).str();
std::string req_fov =
(boost::format("vset /camera/%i/horizontal_fieldofview %.5f")
% cam_id % hfov_deg)
.str();
sendCommand(req_fov);
}
}
void UnrealCvClient::sendCommand(std::string command)
{
if (!(boost::starts_with(command, "vset") || boost::starts_with(command, "vrun"))) {
void UnrealCvClient::sendCommand(std::string command) {
if (!(boost::starts_with(command, "vset")
|| boost::starts_with(command, "vrun"))) {
throw std::runtime_error(
"invalid command: command must start with vget or (vset, vrun)");
"invalid command: command must start with vget or (vset, vrun)"
);
}
uint32_t tmpc = counter_++;
@ -187,27 +197,29 @@ void UnrealCvClient::sendCommand(std::string command)
*/
std::string result = receive<std::string>(
[this] (std::istream& stream, uint32_t size) -> std::string {
[this](std::istream& stream, uint32_t size) -> std::string {
return this->istreamToString(stream, size);
});
}
);
if (!boost::starts_with(result, result_prefix + "ok")) {
throw std::runtime_error("response identifier is incorrect");
} else {
VLOG(1) << "GET:" << tmpc << " " << "ok";
VLOG(1) << "GET:" << tmpc << " "
<< "ok";
}
sleep(delay_);
}
}
template<typename Result>
Result UnrealCvClient::sendCommand(std::string command, std::function<Result(std::istream&, uint32_t)> mapf)
{
if (!(boost::starts_with(command, "vget")))
{
template <typename Result>
Result UnrealCvClient::sendCommand(
std::string command, std::function<Result(std::istream&, uint32_t)> mapf
) {
if (!(boost::starts_with(command, "vget"))) {
throw std::runtime_error(
"invalid command: command must start with vget or (vset, vrun)");
"invalid command: command must start with vget or (vset, vrun)"
);
}
uint32_t tmpc = counter_++;
@ -223,24 +235,26 @@ Result UnrealCvClient::sendCommand(std::string command, std::function<Result(std
*/
Result result = receive<Result>(
[this, result_prefix, mapf] (std::istream& stream, uint32_t size) -> Result {
[this,
result_prefix,
mapf](std::istream& stream, uint32_t size) -> Result {
std::string prefix =
istreamToString(stream, result_prefix.size());
size -= result_prefix.size();
std::string prefix = istreamToString(stream, result_prefix.size());
size-=result_prefix.size();
if(!boost::starts_with(prefix, result_prefix)) {
throw std::runtime_error("response identifier is incorrect");
}
if (!boost::starts_with(prefix, result_prefix))
throw std::runtime_error("response identifier is incorrect"
);
return mapf(stream, size);
});
}
);
sleep(delay_);
return result;
}
}
void UnrealCvClient::send(std::string msg, uint32_t counter)
{
void UnrealCvClient::send(std::string msg, uint32_t counter) {
std::string out = std::to_string(counter) + ":" + msg;
uint32_t magic = 0x9E2B83C1;
@ -255,21 +269,30 @@ void UnrealCvClient::send(std::string msg, uint32_t counter)
request_stream << out;
// Send the request.
boost::asio::write(socket_,
boost::asio::write(
socket_,
request,
boost::asio::transfer_exactly(request.size() + sizeof(magic) + sizeof(size)),
ec);
}
template<typename Result>
Result UnrealCvClient::receive(std::function<Result(std::istream&, uint32_t)> parser)
{
boost::asio::transfer_exactly(
request.size() + sizeof(magic) + sizeof(size)
),
ec
);
}
template <typename Result>
Result UnrealCvClient::receive(
std::function<Result(std::istream&, uint32_t)> parser
) {
boost::system::error_code ec;
boost::asio::streambuf response;
//first read the 8 byte header
boost::asio::read(socket_, response, boost::asio::transfer_exactly(8), ec);
// first read the 8 byte header
boost::asio::read(
socket_,
response,
boost::asio::transfer_exactly(8),
ec
);
handleError(ec);
uint32_t magic;
@ -277,35 +300,34 @@ Result UnrealCvClient::receive(std::function<Result(std::istream&, uint32_t)> p
// Check that response is OK.
std::istream response_stream(&response);
response_stream.read((char*)&magic, sizeof(magic));
response_stream.read((char*)&size, sizeof(size));
response_stream.read((char*) &magic, sizeof(magic));
response_stream.read((char*) &size, sizeof(size));
boost::asio::read(socket_, response, boost::asio::transfer_exactly(size), ec);
boost::asio::read(
socket_,
response,
boost::asio::transfer_exactly(size),
ec
);
handleError(ec);
Result res = parser(response_stream, size);
return res;
}
}
void UnrealCvClient::handleError(boost::system::error_code ec)
{
if (ec == boost::asio::error::eof) {
void UnrealCvClient::handleError(boost::system::error_code ec) {
if (ec == boost::asio::error::eof)
throw boost::system::system_error(ec); // Some other error.
} else if (ec) {
else if (ec)
throw boost::system::system_error(ec); // Some other error.
}
}
void UnrealCvClient::sleep(uint32_t delay) {
void UnrealCvClient::sleep(uint32_t delay) {
std::this_thread::sleep_for(std::chrono::milliseconds(delay));
}
std::string UnrealCvClient::istreamToString(
std::istream& stream, uint32_t size)
{
}
std::string
UnrealCvClient::istreamToString(std::istream& stream, uint32_t size) {
char buffer[size];
stream.read(buffer, size);
@ -313,28 +335,28 @@ std::string UnrealCvClient::istreamToString(
out << buffer;
std::string result = out.str();
return result;
}
}
// from cnpy: https://github.com/rogersce/cnpy/blob/master/cnpy.cpp
void UnrealCvClient::parse_npy_header(unsigned char* buffer,
// from cnpy: https://github.com/rogersce/cnpy/blob/master/cnpy.cpp
void UnrealCvClient::parse_npy_header(
unsigned char* buffer,
size_t& word_size,
std::vector<size_t>& shape,
bool& fortran_order)
{
//std::string magic_string(buffer,6);
uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer+6);
uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer+7);
uint16_t header_len = *reinterpret_cast<uint16_t*>(buffer+8);
std::string header(reinterpret_cast<char*>(buffer+9),header_len);
bool& fortran_order
) {
// std::string magic_string(buffer,6);
uint8_t major_version = *reinterpret_cast<uint8_t*>(buffer + 6);
uint8_t minor_version = *reinterpret_cast<uint8_t*>(buffer + 7);
uint16_t header_len = *reinterpret_cast<uint16_t*>(buffer + 8);
std::string header(reinterpret_cast<char*>(buffer + 9), header_len);
size_t loc1, loc2;
//fortran order
loc1 = header.find("fortran_order")+16;
fortran_order = (header.substr(loc1,4) == "True" ? true : false);
// fortran order
loc1 = header.find("fortran_order") + 16;
fortran_order = (header.substr(loc1, 4) == "True" ? true : false);
//shape
// shape
loc1 = header.find("(");
loc2 = header.find(")");
@ -342,27 +364,26 @@ void UnrealCvClient::parse_npy_header(unsigned char* buffer,
std::smatch sm;
shape.clear();
std::string str_shape = header.substr(loc1+1,loc2-loc1-1);
while(std::regex_search(str_shape, sm, num_regex))
{
std::string str_shape = header.substr(loc1 + 1, loc2 - loc1 - 1);
while (std::regex_search(str_shape, sm, num_regex)) {
shape.push_back(std::stoi(sm[0].str()));
str_shape = sm.suffix().str();
}
//endian, word size, data type
//byte order code | stands for not applicable.
//not sure when this applies except for byte array
loc1 = header.find("descr")+9;
bool littleEndian = (header[loc1] == '<' || header[loc1] == '|' ? true : false);
// endian, word size, data type
// byte order code | stands for not applicable.
// not sure when this applies except for byte array
loc1 = header.find("descr") + 9;
bool littleEndian =
(header[loc1] == '<' || header[loc1] == '|' ? true : false);
assert(littleEndian);
//char type = header[loc1+1];
//assert(type == map_type(T));
// char type = header[loc1+1];
// assert(type == map_type(T));
std::string str_ws = header.substr(loc1+2);
std::string str_ws = header.substr(loc1 + 2);
loc2 = str_ws.find("'");
word_size = atoi(str_ws.substr(0,loc2).c_str());
}
word_size = atoi(str_ws.substr(0, loc2).c_str());
}
} // namespace event_camera_simulator

16
event_camera_simulator/esim_unrealcv_bridge/test/test_unrealcv_bridge.cpp
View File

@ -7,18 +7,12 @@ using namespace std;
int main() {
event_camera_simulator::UnrealCvClient client("localhost", "9000");
cv::namedWindow("Image", cv::WINDOW_AUTOSIZE );
cv::namedWindow("Depthmap", cv::WINDOW_AUTOSIZE );
cv::namedWindow("Image", cv::WINDOW_AUTOSIZE);
cv::namedWindow("Depthmap", cv::WINDOW_AUTOSIZE);
for(double y = 0.0; y<100.0; y+=10.0)
{
event_camera_simulator::CameraData test = {0,
0.0,
0.0,
0.0,
0.0,
y,
100.0};
for (double y = 0.0; y < 100.0; y += 10.0) {
event_camera_simulator::CameraData test =
{0, 0.0, 0.0, 0.0, 0.0, y, 100.0};
client.setCamera(test);
cv::Mat img = client.getImage(0);

62
event_camera_simulator/esim_visualization/include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp
View File

@ -2,44 +2,68 @@
#include <esim/common/types.hpp>
#include <esim/visualization/publisher_interface.hpp>
#include <fstream>
namespace event_camera_simulator {
class AdaptiveSamplingBenchmarkPublisher : public Publisher
{
public:
using PixelLocation = std::pair<int,int>;
class AdaptiveSamplingBenchmarkPublisher : public Publisher {
public:
using PixelLocation = std::pair<int, int>;
using PixelLocations = std::vector<PixelLocation>;
AdaptiveSamplingBenchmarkPublisher(const std::string &benchmark_folder,
const std::string &pixels_to_record_filename);
AdaptiveSamplingBenchmarkPublisher(
const std::string& benchmark_folder,
const std::string& pixels_to_record_filename
);
~AdaptiveSamplingBenchmarkPublisher();
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
virtual void
imageCallback(const ImagePtrVector& images, Time t) override;
virtual void eventsCallback(const EventsVector& events) override;
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override;
virtual void opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) override;
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override {}
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) override {}
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) override {}
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override {}
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) override {}
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) override {}
virtual void imageCorruptedCallback(
const ImagePtrVector& corrupted_images, Time t
) override {}
virtual void
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
virtual void poseCallback(
const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t
) override {}
virtual void twistCallback(
const AngularVelocityVector& ws,
const LinearVelocityVector& vs,
Time t
) override {}
virtual void
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override {}
virtual void cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) override {}
virtual void pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) override {}
static Publisher::Ptr createFromGflags();
private:
private:
std::ofstream events_file_;
std::ofstream images_file_;
std::ofstream pixel_intensities_file_;
std::ofstream optic_flows_file_;
size_t image_index_;
PixelLocations pixels_to_record_;
};
};
} // namespace event_camera_simulator

48
event_camera_simulator/esim_visualization/include/esim/visualization/publisher_interface.hpp
View File

@ -5,25 +5,47 @@
namespace event_camera_simulator {
class Publisher
{
public:
class Publisher {
public:
ZE_POINTER_TYPEDEFS(Publisher);
Publisher() = default;
virtual ~Publisher() = default;
virtual void imageCallback(const ImagePtrVector& images, Time t) {}
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) {}
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {}
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) {}
virtual void eventsCallback(const EventsVector& events) {}
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) {}
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) {}
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {}
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) {}
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) {}
};
virtual void
imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) {
}
virtual void
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {}
virtual void
opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) {}
virtual void eventsCallback(const EventsVector& events) {}
virtual void poseCallback(
const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t
) {}
virtual void twistCallback(
const AngularVelocityVector& ws,
const LinearVelocityVector& vs,
Time t
) {}
virtual void
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {}
virtual void
cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) {}
virtual void
pointcloudCallback(const PointCloudVector& pointclouds, Time t) {}
};
} // namespace event_camera_simulator

69
event_camera_simulator/esim_visualization/include/esim/visualization/ros_publisher.hpp
View File

@ -2,49 +2,67 @@
#include <esim/common/types.hpp>
#include <esim/visualization/publisher_interface.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
namespace event_camera_simulator {
class RosPublisher : public Publisher
{
public:
class RosPublisher : public Publisher {
public:
RosPublisher(size_t num_cameras);
~RosPublisher();
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override;
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override;
virtual void
imageCallback(const ImagePtrVector& images, Time t) override;
virtual void imageCorruptedCallback(
const ImagePtrVector& corrupted_images, Time t
) override;
virtual void
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
virtual void opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) override;
virtual void eventsCallback(const EventsVector& events) override;
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) override;
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) override;
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override;
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) override;
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) override;
virtual void poseCallback(
const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t
) override;
virtual void twistCallback(
const AngularVelocityVector& ws,
const LinearVelocityVector& vs,
Time t
) override;
virtual void
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override;
virtual void cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) override;
virtual void pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) override;
private:
private:
size_t num_cameras_;
std::vector<cv::Size> sensor_sizes_;
std::shared_ptr<ros::NodeHandle> nh_;
std::shared_ptr<image_transport::ImageTransport> it_;
std::vector< std::shared_ptr<ros::Publisher> > event_pub_;
std::vector<std::shared_ptr<ros::Publisher>> event_pub_;
std::shared_ptr<ros::Publisher> pose_pub_;
std::shared_ptr<ros::Publisher> imu_pub_;
std::vector< std::shared_ptr<ros::Publisher> > pointcloud_pub_;
std::vector< std::shared_ptr<ros::Publisher> > camera_info_pub_;
std::vector< std::shared_ptr<image_transport::Publisher> > image_pub_;
std::vector< std::shared_ptr<image_transport::Publisher> > image_corrupted_pub_;
std::vector< std::shared_ptr<image_transport::Publisher> > depthmap_pub_;
std::vector< std::shared_ptr<ros::Publisher> > optic_flow_pub_;
std::vector< std::shared_ptr<ros::Publisher> > twist_pub_;
std::vector<std::shared_ptr<ros::Publisher>> pointcloud_pub_;
std::vector<std::shared_ptr<ros::Publisher>> camera_info_pub_;
std::vector<std::shared_ptr<image_transport::Publisher>> image_pub_;
std::vector<std::shared_ptr<image_transport::Publisher>>
image_corrupted_pub_;
std::vector<std::shared_ptr<image_transport::Publisher>> depthmap_pub_;
std::vector<std::shared_ptr<ros::Publisher>> optic_flow_pub_;
std::vector<std::shared_ptr<ros::Publisher>> twist_pub_;
std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;
Time last_published_camera_info_time_;
@ -53,7 +71,6 @@ private:
Time last_published_depthmap_time_;
Time last_published_optic_flow_time_;
Time last_published_pointcloud_time_;
};
};
} // namespace event_camera_simulator

60
event_camera_simulator/esim_visualization/include/esim/visualization/ros_utils.hpp
View File

@ -1,54 +1,66 @@
#pragma once
#include <esim/common/types.hpp>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/Image.h>
#include <dvs_msgs/EventArray.h>
#include <esim/common/types.hpp>
#include <esim_msgs/OpticFlow.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/Imu.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/CameraInfo.h>
#include <esim_msgs/OpticFlow.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
namespace event_camera_simulator {
inline std::string getTopicName(int i, const std::string& suffix)
{
inline std::string getTopicName(int i, const std::string& suffix) {
std::stringstream ss;
ss << "cam" << i << "/" << suffix;
return ss.str();
}
}
inline std::string getTopicName(const std::string& prefix, int i, const std::string& suffix)
{
inline std::string
getTopicName(const std::string& prefix, int i, const std::string& suffix) {
std::stringstream ss;
ss << prefix << "/" << getTopicName(i, suffix);
return ss.str();
}
}
inline ros::Time toRosTime(Time t)
{
inline ros::Time toRosTime(Time t) {
ros::Time ros_time;
ros_time.fromNSec(t);
return ros_time;
}
}
void pointCloudToMsg(const PointCloud& pointcloud, const std::string& frame_id, Time t,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg);
void pointCloudToMsg(
const PointCloud& pointcloud,
const std::string& frame_id,
Time t,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg
);
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg);
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg);
void depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg);
void
depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg);
void opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg);
void
opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg);
void eventsToMsg(const Events& events, int width, int height, dvs_msgs::EventArrayPtr& msg);
void eventsToMsg(
const Events& events,
int width,
int height,
dvs_msgs::EventArrayPtr& msg
);
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t);
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t);
geometry_msgs::TwistStamped twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t);
void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg);
geometry_msgs::TwistStamped
twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t);
void cameraToMsg(
const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg
);
} // namespace event_camera_simulator

50
event_camera_simulator/esim_visualization/include/esim/visualization/rosbag_writer.hpp
View File

@ -6,27 +6,44 @@
namespace event_camera_simulator {
class RosbagWriter : public Publisher
{
public:
RosbagWriter(const std::string& path_to_output_bag,
size_t num_cameras);
class RosbagWriter : public Publisher {
public:
RosbagWriter(const std::string& path_to_output_bag, size_t num_cameras);
~RosbagWriter();
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override;
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override;
virtual void
imageCallback(const ImagePtrVector& images, Time t) override;
virtual void imageCorruptedCallback(
const ImagePtrVector& corrupted_images, Time t
) override;
virtual void
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
virtual void opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) override;
virtual void eventsCallback(const EventsVector& events) override;
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) override;
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) override;
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override;
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) override;
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) override;
virtual void poseCallback(
const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t
) override;
virtual void twistCallback(
const AngularVelocityVector& ws,
const LinearVelocityVector& vs,
Time t
) override;
virtual void
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override;
virtual void cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) override;
virtual void pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) override;
static Publisher::Ptr createBagWriterFromGflags(size_t num_cameras);
private:
private:
size_t num_cameras_;
std::vector<cv::Size> sensor_sizes_;
rosbag::Bag bag_;
@ -39,7 +56,6 @@ private:
Time last_published_depthmap_time_;
Time last_published_optic_flow_time_;
Time last_published_pointcloud_time_;
};
};
} // namespace event_camera_simulator

60
event_camera_simulator/esim_visualization/include/esim/visualization/synthetic_optic_flow_publisher.hpp
View File

@ -2,44 +2,66 @@
#include <esim/common/types.hpp>
#include <esim/visualization/publisher_interface.hpp>
#include <fstream>
namespace event_camera_simulator {
class SyntheticOpticFlowPublisher : public Publisher
{
public:
SyntheticOpticFlowPublisher(const std::string &output_folder);
class SyntheticOpticFlowPublisher : public Publisher {
public:
SyntheticOpticFlowPublisher(const std::string& output_folder);
~SyntheticOpticFlowPublisher();
virtual void imageCallback(const ImagePtrVector& images, Time t) override {
virtual void
imageCallback(const ImagePtrVector& images, Time t) override {
CHECK_EQ(images.size(), 1);
if(sensor_size_.width == 0 || sensor_size_.height == 0)
{
if (sensor_size_.width == 0 || sensor_size_.height == 0)
sensor_size_ = images[0]->size();
}
}
virtual void eventsCallback(const EventsVector& events) override;
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override {}
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override {}
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) override {}
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) override {}
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override {}
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) override {}
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) override {}
virtual void opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) override {}
virtual void imageCorruptedCallback(
const ImagePtrVector& corrupted_images, Time t
) override {}
virtual void
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
virtual void poseCallback(
const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t
) override {}
virtual void twistCallback(
const AngularVelocityVector& ws,
const LinearVelocityVector& vs,
Time t
) override {}
virtual void
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override {}
virtual void cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) override {}
virtual void pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) override {}
static Publisher::Ptr createFromGflags();
private:
private:
std::string output_folder_;
cv::Size sensor_size_;
std::ofstream events_file_;
Events events_; // buffer containing all the events since the beginning
};
};
} // namespace event_camera_simulator

158
event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp
View File

@ -1,78 +1,93 @@
#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
#include <esim/common/utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/time_conversions.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui/highgui.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/time_conversions.hpp>
DEFINE_string(adaptive_sampling_benchmark_folder, "",
"Folder in which to output the results.");
DEFINE_string(
adaptive_sampling_benchmark_folder,
"",
"Folder in which to output the results."
);
DEFINE_string(adaptive_sampling_benchmark_pixels_to_record_file, "",
"File containing the pixel locations to record.");
DEFINE_string(
adaptive_sampling_benchmark_pixels_to_record_file,
"",
"File containing the pixel locations to record."
);
namespace event_camera_simulator {
AdaptiveSamplingBenchmarkPublisher::AdaptiveSamplingBenchmarkPublisher(const std::string& benchmark_folder,
const std::string& pixels_to_record_filename)
: image_index_(0)
{
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "events.txt"),
&events_file_);
AdaptiveSamplingBenchmarkPublisher::AdaptiveSamplingBenchmarkPublisher(
const std::string& benchmark_folder,
const std::string& pixels_to_record_filename
)
: image_index_(0) {
ze::openOutputFileStream(
ze::joinPath(benchmark_folder, "events.txt"),
&events_file_
);
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "images.txt"),
&images_file_);
ze::openOutputFileStream(
ze::joinPath(benchmark_folder, "images.txt"),
&images_file_
);
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "pixel_intensities.txt"),
&pixel_intensities_file_);
ze::openOutputFileStream(
ze::joinPath(benchmark_folder, "pixel_intensities.txt"),
&pixel_intensities_file_
);
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "optic_flows.txt"),
&optic_flows_file_);
ze::openOutputFileStream(
ze::joinPath(benchmark_folder, "optic_flows.txt"),
&optic_flows_file_
);
// Load and parse the file containing the list of pixel locations
// whose intensity values to record
std::ifstream pixels_to_record_file;
if(pixels_to_record_filename != "")
{
ze::openFileStream(pixels_to_record_filename, &pixels_to_record_file);
if (pixels_to_record_filename != "") {
ze::openFileStream(
pixels_to_record_filename,
&pixels_to_record_file
);
int x, y;
LOG(INFO) << "Pixels that will be recorded: ";
while(pixels_to_record_file >> x >> y)
{
while (pixels_to_record_file >> x >> y) {
LOG(INFO) << x << " , " << y;
pixels_to_record_.push_back(PixelLocation(x,y));
pixels_to_record_.push_back(PixelLocation(x, y));
}
}
}
}
Publisher::Ptr AdaptiveSamplingBenchmarkPublisher::createFromGflags()
{
if(FLAGS_adaptive_sampling_benchmark_folder == "")
{
LOG(WARNING) << "Empty benchmark folder string: will not write benchmark files";
Publisher::Ptr AdaptiveSamplingBenchmarkPublisher::createFromGflags() {
if (FLAGS_adaptive_sampling_benchmark_folder == "") {
LOG(WARNING) << "Empty benchmark folder string: will not write "
"benchmark files";
return nullptr;
}
return std::make_shared<AdaptiveSamplingBenchmarkPublisher>(FLAGS_adaptive_sampling_benchmark_folder,
FLAGS_adaptive_sampling_benchmark_pixels_to_record_file);
}
return std::make_shared<AdaptiveSamplingBenchmarkPublisher>(
FLAGS_adaptive_sampling_benchmark_folder,
FLAGS_adaptive_sampling_benchmark_pixels_to_record_file
);
}
AdaptiveSamplingBenchmarkPublisher::~AdaptiveSamplingBenchmarkPublisher()
{
AdaptiveSamplingBenchmarkPublisher::~AdaptiveSamplingBenchmarkPublisher() {
// finish writing files
events_file_.close();
images_file_.close();
pixel_intensities_file_.close();
optic_flows_file_.close();
}
}
void AdaptiveSamplingBenchmarkPublisher::imageCallback(const ImagePtrVector& images, Time t)
{
void AdaptiveSamplingBenchmarkPublisher::imageCallback(
const ImagePtrVector& images, Time t
) {
CHECK_EQ(images.size(), 1);
images_file_ << t << std::endl;
@ -80,56 +95,57 @@ void AdaptiveSamplingBenchmarkPublisher::imageCallback(const ImagePtrVector& ima
cv::Mat img_8bit;
img->convertTo(img_8bit, CV_8U, 255);
if(image_index_ == 0)
{
static const std::vector<int> compression_params = {cv::IMWRITE_PNG_COMPRESSION, 0};
if (image_index_ == 0) {
static const std::vector<int> compression_params = {
cv::IMWRITE_PNG_COMPRESSION,
0};
std::stringstream ss;
ss << ze::joinPath(FLAGS_adaptive_sampling_benchmark_folder, "image_");
ss << ze::joinPath(
FLAGS_adaptive_sampling_benchmark_folder,
"image_"
);
ss << image_index_ << ".png";
LOG(INFO) << ss.str();
cv::imwrite(ss.str(), img_8bit, compression_params);
}
for(const PixelLocation& pixel_loc : pixels_to_record_)
{
for (const PixelLocation& pixel_loc : pixels_to_record_) {
// write line in the form "x y I(x,y)"
const int x = pixel_loc.first;
const int y = pixel_loc.second;
pixel_intensities_file_ << x << " "
<< y << " "
<< (*images[0])(y,x) << std::endl;
pixel_intensities_file_ << x << " " << y << " "
<< (*images[0])(y, x) << std::endl;
}
image_index_++;
}
}
void AdaptiveSamplingBenchmarkPublisher::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
{
void AdaptiveSamplingBenchmarkPublisher::opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) {
CHECK_EQ(optic_flows.size(), 1);
for(const PixelLocation& pixel_loc : pixels_to_record_)
{
// write line in the form "x y optic_flow(x,y)[0] optic_flow(x,y)[1]"
for (const PixelLocation& pixel_loc : pixels_to_record_) {
// write line in the form "x y optic_flow(x,y)[0]
// optic_flow(x,y)[1]"
const int x = pixel_loc.first;
const int y = pixel_loc.second;
optic_flows_file_ << x << " "
<< y << " "
<< (*optic_flows[0])(y,x)[0] << " "
<< (*optic_flows[0])(y,x)[1]
<< std::endl;
optic_flows_file_ << x << " " << y << " "
<< (*optic_flows[0])(y, x)[0] << " "
<< (*optic_flows[0])(y, x)[1] << std::endl;
}
}
}
void AdaptiveSamplingBenchmarkPublisher::eventsCallback(const EventsVector& events)
{
void AdaptiveSamplingBenchmarkPublisher::eventsCallback(
const EventsVector& events
) {
CHECK_EQ(events.size(), 1);
for(const Event& e : events[0])
{
events_file_ << e.t << " " << e.x << " " << e.y << " " << (e.pol? 1 : 0) << std::endl;
for (const Event& e : events[0]) {
events_file_ << e.t << " " << e.x << " " << e.y << " "
<< (e.pol ? 1 : 0) << std::endl;
}
}
}
} // namespace event_camera_simulator

401
event_camera_simulator/esim_visualization/src/ros_publisher.cpp
View File

@ -1,86 +1,116 @@
#include <esim/visualization/ros_publisher.hpp>
#include <esim/common/utils.hpp>
#include <ze/common/time_conversions.hpp>
#include <esim/visualization/ros_publisher.hpp>
#include <esim/visualization/ros_utils.hpp>
#include <minkindr_conversions/kindr_msg.h>
#include <minkindr_conversions/kindr_tf.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <minkindr_conversions/kindr_msg.h>
#include <minkindr_conversions/kindr_tf.h>
#include <ze/common/time_conversions.hpp>
DEFINE_double(ros_publisher_camera_info_rate, 0,
"Camera info (maximum) publish rate, in Hz");
DEFINE_double(
ros_publisher_camera_info_rate,
0,
"Camera info (maximum) publish rate, in Hz"
);
DEFINE_double(ros_publisher_frame_rate, 30,
"(Maximum) frame rate, in Hz");
DEFINE_double(ros_publisher_frame_rate, 30, "(Maximum) frame rate, in Hz");
DEFINE_double(ros_publisher_depth_rate, 0,
"(Maximum) depthmap publish rate, in Hz");
DEFINE_double(
ros_publisher_depth_rate, 0, "(Maximum) depthmap publish rate, in Hz"
);
DEFINE_double(ros_publisher_pointcloud_rate, 0,
"(Maximum) point cloud publish rate, in Hz");
DEFINE_double(
ros_publisher_pointcloud_rate,
0,
"(Maximum) point cloud publish rate, in Hz"
);
DEFINE_double(ros_publisher_optic_flow_rate, 0,
"(Maximum) optic flow map publish rate, in Hz");
DEFINE_double(
ros_publisher_optic_flow_rate,
0,
"(Maximum) optic flow map publish rate, in Hz"
);
namespace event_camera_simulator {
RosPublisher::RosPublisher(size_t num_cameras)
{
RosPublisher::RosPublisher(size_t num_cameras) {
CHECK_GE(num_cameras, 1);
num_cameras_ = num_cameras;
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
// Initialize ROS if it was not initialized before.
if(!ros::isInitialized())
{
if (!ros::isInitialized()) {
VLOG(1) << "Initializing ROS";
int argc = 0;
ros::init(argc, nullptr, std::string("ros_publisher"), ros::init_options::NoSigintHandler);
ros::init(
argc,
nullptr,
std::string("ros_publisher"),
ros::init_options::NoSigintHandler
);
}
// Create node and subscribe.
nh_.reset(new ros::NodeHandle(""));
it_.reset(new image_transport::ImageTransport(*nh_));
// Setup ROS publishers for images, events, poses, depth maps, camera info, etc.
for(size_t i=0; i<num_cameras_; ++i)
{
// Setup ROS publishers for images, events, poses, depth maps, camera
// info, etc.
for (size_t i = 0; i < num_cameras_; ++i) {
event_pub_.emplace_back(
new ros::Publisher(
nh_->advertise<dvs_msgs::EventArray> (getTopicName(i, "events"), 0)));
new ros::Publisher(nh_->advertise<dvs_msgs::EventArray>(
getTopicName(i, "events"),
0
))
);
image_pub_.emplace_back(
new image_transport::Publisher(
it_->advertise(getTopicName(i, "image_raw"), 0)));
image_pub_.emplace_back(new image_transport::Publisher(
it_->advertise(getTopicName(i, "image_raw"), 0)
));
image_corrupted_pub_.emplace_back(
new image_transport::Publisher(
it_->advertise(getTopicName(i, "image_corrupted"), 0)));
image_corrupted_pub_.emplace_back(new image_transport::Publisher(
it_->advertise(getTopicName(i, "image_corrupted"), 0)
));
depthmap_pub_.emplace_back(
new image_transport::Publisher(
it_->advertise(getTopicName(i, "depthmap"), 0)));
depthmap_pub_.emplace_back(new image_transport::Publisher(
it_->advertise(getTopicName(i, "depthmap"), 0)
));
optic_flow_pub_.emplace_back(
new ros::Publisher(
nh_->advertise<esim_msgs::OpticFlow> (getTopicName(i, "optic_flow"), 0)));
new ros::Publisher(nh_->advertise<esim_msgs::OpticFlow>(
getTopicName(i, "optic_flow"),
0
))
);
camera_info_pub_.emplace_back(
new ros::Publisher(
nh_->advertise<sensor_msgs::CameraInfo> (getTopicName(i, "camera_info"), 0)));
new ros::Publisher(nh_->advertise<sensor_msgs::CameraInfo>(
getTopicName(i, "camera_info"),
0
))
);
twist_pub_.emplace_back(
new ros::Publisher(
nh_->advertise<geometry_msgs::TwistStamped> (getTopicName(i, "twist"), 0)));
new ros::Publisher(nh_->advertise<geometry_msgs::TwistStamped>(
getTopicName(i, "twist"),
0
))
);
pointcloud_pub_.emplace_back(
new ros::Publisher(
nh_->advertise<pcl::PointCloud<pcl::PointXYZ>> (getTopicName(i, "pointcloud"), 0)));
pointcloud_pub_.emplace_back(new ros::Publisher(
nh_->advertise<pcl::PointCloud<pcl::PointXYZ>>(
getTopicName(i, "pointcloud"),
0
)
));
}
pose_pub_.reset(new ros::Publisher(nh_->advertise<geometry_msgs::PoseStamped> ("pose", 0)));
imu_pub_.reset(new ros::Publisher(nh_->advertise<sensor_msgs::Imu> ("imu", 0)));
pose_pub_.reset(new ros::Publisher(
nh_->advertise<geometry_msgs::PoseStamped>("pose", 0)
));
imu_pub_.reset(
new ros::Publisher(nh_->advertise<sensor_msgs::Imu>("imu", 0))
);
tf_broadcaster_.reset(new tf::TransformBroadcaster());
last_published_camera_info_time_ = 0;
@ -89,12 +119,10 @@ RosPublisher::RosPublisher(size_t num_cameras)
last_published_depthmap_time_ = 0;
last_published_optic_flow_time_ = 0;
last_published_pointcloud_time_ = 0;
}
}
RosPublisher::~RosPublisher()
{
for(size_t i=0; i<num_cameras_; ++i)
{
RosPublisher::~RosPublisher() {
for (size_t i = 0; i < num_cameras_; ++i) {
event_pub_[i]->shutdown();
image_pub_[i]->shutdown();
image_corrupted_pub_[i]->shutdown();
@ -107,62 +135,59 @@ RosPublisher::~RosPublisher()
pose_pub_->shutdown();
ros::shutdown();
}
}
void RosPublisher::pointcloudCallback(const PointCloudVector& pointclouds, Time t)
{
void RosPublisher::pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) {
CHECK_EQ(pointcloud_pub_.size(), num_cameras_);
CHECK_EQ(pointclouds.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
const PointCloud& pcl_camera = pointclouds[i];
CHECK(pointcloud_pub_[i]);
if(pointcloud_pub_[i]->getNumSubscribers() == 0)
{
if (pointcloud_pub_[i]->getNumSubscribers() == 0)
continue;
}
Duration min_time_interval_between_published_pointclouds_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
if(last_published_pointcloud_time_ > 0 && t - last_published_pointcloud_time_ < min_time_interval_between_published_pointclouds_)
{
Duration min_time_interval_between_published_pointclouds_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
if (last_published_pointcloud_time_ > 0
&& t - last_published_pointcloud_time_
< min_time_interval_between_published_pointclouds_) {
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGB>);
std::stringstream ss; ss << "cam" << i;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
msg(new pcl::PointCloud<pcl::PointXYZRGB>);
std::stringstream ss;
ss << "cam" << i;
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
pointcloud_pub_[i]->publish(msg);
}
last_published_pointcloud_time_ = t;
}
}
void RosPublisher::imageCallback(const ImagePtrVector& images, Time t)
{
void RosPublisher::imageCallback(const ImagePtrVector& images, Time t) {
CHECK_EQ(image_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
sensor_sizes_[i] = images[i]->size();
CHECK(image_pub_[i]);
if(image_pub_[i]->getNumSubscribers() == 0)
{
if (image_pub_[i]->getNumSubscribers() == 0)
continue;
}
static const Duration min_time_interval_between_published_images_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if(last_published_image_time_ > 0 && t - last_published_image_time_ < min_time_interval_between_published_images_)
{
static const Duration min_time_interval_between_published_images_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if (last_published_image_time_ > 0
&& t - last_published_image_time_
< min_time_interval_between_published_images_) {
return;
}
if(images[i])
{
if (images[i]) {
sensor_msgs::ImagePtr msg;
imageToMsg(*images[i], t, msg);
image_pub_[i]->publish(msg);
@ -170,29 +195,27 @@ void RosPublisher::imageCallback(const ImagePtrVector& images, Time t)
}
last_published_image_time_ = t;
}
void RosPublisher::imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t)
{
CHECK_EQ(image_corrupted_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
CHECK(image_corrupted_pub_[i]);
if(image_corrupted_pub_[i]->getNumSubscribers() == 0)
{
continue;
}
static const Duration min_time_interval_between_published_images_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if(last_published_corrupted_image_time_ > 0 && t - last_published_corrupted_image_time_ < min_time_interval_between_published_images_)
{
void RosPublisher::imageCorruptedCallback(
const ImagePtrVector& corrupted_images, Time t
) {
CHECK_EQ(image_corrupted_pub_.size(), num_cameras_);
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(image_corrupted_pub_[i]);
if (image_corrupted_pub_[i]->getNumSubscribers() == 0)
continue;
static const Duration min_time_interval_between_published_images_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if (last_published_corrupted_image_time_ > 0
&& t - last_published_corrupted_image_time_
< min_time_interval_between_published_images_) {
return;
}
if(corrupted_images[i])
{
if (corrupted_images[i]) {
sensor_msgs::ImagePtr msg;
imageToMsg(*corrupted_images[i], t, msg);
image_corrupted_pub_[i]->publish(msg);
@ -200,29 +223,27 @@ void RosPublisher::imageCorruptedCallback(const ImagePtrVector& corrupted_images
}
last_published_corrupted_image_time_ = t;
}
void RosPublisher::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t)
{
CHECK_EQ(depthmap_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
CHECK(depthmap_pub_[i]);
if(depthmap_pub_[i]->getNumSubscribers() == 0)
{
continue;
}
static const Duration min_time_interval_between_published_depthmaps_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
if(last_published_depthmap_time_ > 0 && t - last_published_depthmap_time_ < min_time_interval_between_published_depthmaps_)
{
void
RosPublisher::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {
CHECK_EQ(depthmap_pub_.size(), num_cameras_);
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(depthmap_pub_[i]);
if (depthmap_pub_[i]->getNumSubscribers() == 0)
continue;
static const Duration
min_time_interval_between_published_depthmaps_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
if (last_published_depthmap_time_ > 0
&& t - last_published_depthmap_time_
< min_time_interval_between_published_depthmaps_) {
return;
}
if(depthmaps[i])
{
if (depthmaps[i]) {
sensor_msgs::ImagePtr msg;
depthmapToMsg(*depthmaps[i], t, msg);
depthmap_pub_[i]->publish(msg);
@ -230,30 +251,33 @@ void RosPublisher::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t)
}
last_published_depthmap_time_ = t;
}
void RosPublisher::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
{
CHECK_EQ(optic_flow_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
CHECK(optic_flow_pub_[i]);
if(optic_flow_pub_[i]->getNumSubscribers() == 0)
{
continue;
}
static const Duration min_time_interval_between_published_optic_flows_
= (min_time_interval_between_published_optic_flows_ > 0) ? ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate) : 0;
if(min_time_interval_between_published_optic_flows_ > 0 && last_published_optic_flow_time_ > 0 && t - last_published_optic_flow_time_ < min_time_interval_between_published_optic_flows_)
{
void RosPublisher::opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) {
CHECK_EQ(optic_flow_pub_.size(), num_cameras_);
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(optic_flow_pub_[i]);
if (optic_flow_pub_[i]->getNumSubscribers() == 0)
continue;
static const Duration
min_time_interval_between_published_optic_flows_ =
(min_time_interval_between_published_optic_flows_ > 0)
? ze::secToNanosec(
1.0 / FLAGS_ros_publisher_optic_flow_rate
)
: 0;
if (min_time_interval_between_published_optic_flows_ > 0
&& last_published_optic_flow_time_ > 0
&& t - last_published_optic_flow_time_
< min_time_interval_between_published_optic_flows_) {
return;
}
if(optic_flows[i])
{
if (optic_flows[i]) {
esim_msgs::OpticFlow::Ptr msg;
msg.reset(new esim_msgs::OpticFlow);
opticFlowToMsg(*optic_flows[i], t, msg);
@ -262,44 +286,40 @@ void RosPublisher::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time
}
last_published_optic_flow_time_ = t;
}
}
void RosPublisher::eventsCallback(const EventsVector& events)
{
void RosPublisher::eventsCallback(const EventsVector& events) {
CHECK_EQ(event_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
if(sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
{
for (size_t i = 0; i < num_cameras_; ++i) {
if (sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
continue;
}
if(events[i].empty())
{
if (events[i].empty())
continue;
}
CHECK(event_pub_[i]);
if(event_pub_[i]->getNumSubscribers() == 0)
{
if (event_pub_[i]->getNumSubscribers() == 0)
continue;
}
dvs_msgs::EventArrayPtr msg;
msg.reset(new dvs_msgs::EventArray);
eventsToMsg(events[i], sensor_sizes_[i].width, sensor_sizes_[i].height, msg);
eventsToMsg(
events[i],
sensor_sizes_[i].width,
sensor_sizes_[i].height,
msg
);
event_pub_[i]->publish(msg);
}
}
}
void RosPublisher::poseCallback(const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t)
{
if(T_W_Cs.size() != num_cameras_)
{
LOG(WARNING) << "Number of poses is different than number of cameras."
void RosPublisher::poseCallback(
const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t
) {
if (T_W_Cs.size() != num_cameras_) {
LOG(WARNING
) << "Number of poses is different than number of cameras."
<< "Will not output poses.";
return;
}
@ -312,8 +332,7 @@ void RosPublisher::poseCallback(const Transformation& T_W_B,
tf::transformKindrToTF(T_W_B, &bt);
tf_broadcaster_->sendTransform(bt);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
std::stringstream ss;
ss << "cam" << i;
tf::StampedTransform bt;
@ -326,16 +345,21 @@ void RosPublisher::poseCallback(const Transformation& T_W_B,
// Publish pose message
geometry_msgs::PoseStamped pose_stamped_msg;
tf::poseStampedKindrToMsg(T_W_B, toRosTime(t), "map", &pose_stamped_msg);
tf::poseStampedKindrToMsg(
T_W_B,
toRosTime(t),
"map",
&pose_stamped_msg
);
pose_pub_->publish(pose_stamped_msg);
}
}
void RosPublisher::twistCallback(const AngularVelocityVector &ws, const LinearVelocityVector &vs, Time t)
{
if(ws.size() != num_cameras_
|| vs.size() != num_cameras_)
{
LOG(WARNING) << "Number of twists is different than number of cameras."
void RosPublisher::twistCallback(
const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t
) {
if (ws.size() != num_cameras_ || vs.size() != num_cameras_) {
LOG(WARNING
) << "Number of twists is different than number of cameras."
<< "Will not output twists.";
return;
}
@ -343,49 +367,43 @@ void RosPublisher::twistCallback(const AngularVelocityVector &ws, const LinearVe
CHECK_EQ(vs.size(), num_cameras_);
CHECK_EQ(twist_pub_.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(twist_pub_[i]);
if(twist_pub_[i]->getNumSubscribers() == 0)
{
if (twist_pub_[i]->getNumSubscribers() == 0)
continue;
}
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
twist_pub_[i]->publish(msg);
}
}
void RosPublisher::imuCallback(const Vector3& acc, const Vector3& gyr, Time t)
{
if(imu_pub_->getNumSubscribers() == 0)
{
return;
}
void
RosPublisher::imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {
if (imu_pub_->getNumSubscribers() == 0)
return;
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
imu_pub_->publish(msg);
}
}
void RosPublisher::cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t)
{
void RosPublisher::cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) {
CHECK(camera_rig);
CHECK_EQ(camera_rig->size(), num_cameras_);
static const Duration min_time_interval_between_published_camera_info_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
if(last_published_camera_info_time_ > 0 && t - last_published_camera_info_time_ < min_time_interval_between_published_camera_info_)
{
static const Duration min_time_interval_between_published_camera_info_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
if (last_published_camera_info_time_ > 0
&& t - last_published_camera_info_time_
< min_time_interval_between_published_camera_info_) {
return;
}
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
CHECK(camera_info_pub_[i]);
if(camera_info_pub_[i]->getNumSubscribers() == 0)
{
if (camera_info_pub_[i]->getNumSubscribers() == 0)
continue;
}
sensor_msgs::CameraInfoPtr msg;
msg.reset(new sensor_msgs::CameraInfo);
@ -394,7 +412,6 @@ void RosPublisher::cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time
}
last_published_camera_info_time_ = t;
}
}
} // namespace event_camera_simulator

107
event_camera_simulator/esim_visualization/src/ros_utils.cpp
View File

@ -1,18 +1,21 @@
#include <esim/visualization/ros_utils.hpp>
#include <esim/common/utils.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <esim/common/utils.hpp>
#include <esim/visualization/ros_utils.hpp>
#include <pcl_conversions/pcl_conversions.h>
namespace event_camera_simulator {
void pointCloudToMsg(const PointCloud& pointcloud, const std::string& frame_id, Time t, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg)
{
void pointCloudToMsg(
const PointCloud& pointcloud,
const std::string& frame_id,
Time t,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg
) {
CHECK(msg);
msg->header.frame_id = frame_id;
msg->height = pointcloud.size();
msg->width = 1;
for(auto& p_c : pointcloud)
{
for (auto& p_c : pointcloud) {
pcl::PointXYZRGB p;
p.x = p_c.xyz(0);
p.y = p_c.xyz(1);
@ -24,28 +27,29 @@ void pointCloudToMsg(const PointCloud& pointcloud, const std::string& frame_id,
}
pcl_conversions::toPCL(toRosTime(t), msg->header.stamp);
}
}
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg)
{
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg) {
cv_bridge::CvImage cv_image;
image.convertTo(cv_image.image, CV_8U, 255.0);
cv_image.encoding = "mono8";
cv_image.header.stamp = toRosTime(t);
msg = cv_image.toImageMsg();
}
}
void depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg)
{
void depthmapToMsg(
const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg
) {
cv_bridge::CvImage cv_depthmap;
depthmap.convertTo(cv_depthmap.image, CV_32FC1);
cv_depthmap.encoding = "32FC1";
cv_depthmap.header.stamp = toRosTime(t);
msg = cv_depthmap.toImageMsg();
}
}
void opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg)
{
void opticFlowToMsg(
const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg
) {
CHECK(msg);
msg->header.stamp = toRosTime(t);
@ -56,22 +60,23 @@ void opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg)
msg->flow_x.resize(height * width);
msg->flow_y.resize(height * width);
for(int y=0; y<height; ++y)
{
for(int x=0; x<width; ++x)
{
msg->flow_x[x + y * width] = static_cast<float>(flow(y,x)[0]);
msg->flow_y[x + y * width] = static_cast<float>(flow(y,x)[1]);
for (int y = 0; y < height; ++y) {
for (int x = 0; x < width; ++x) {
msg->flow_x[x + y * width] = static_cast<float>(flow(y, x)[0]);
msg->flow_y[x + y * width] = static_cast<float>(flow(y, x)[1]);
}
}
}
}
void eventsToMsg(const Events& events, int width, int height, dvs_msgs::EventArrayPtr& msg)
{
void eventsToMsg(
const Events& events,
int width,
int height,
dvs_msgs::EventArrayPtr& msg
) {
CHECK(msg);
std::vector<dvs_msgs::Event> events_list;
for(const Event& e : events)
{
for (const Event& e : events) {
dvs_msgs::Event ev;
ev.x = e.x;
ev.y = e.y;
@ -85,10 +90,9 @@ void eventsToMsg(const Events& events, int width, int height, dvs_msgs::EventArr
msg->height = height;
msg->width = width;
msg->header.stamp = events_list.back().ts;
}
}
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t)
{
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t) {
sensor_msgs::Imu imu;
imu.header.stamp = toRosTime(t);
@ -101,10 +105,10 @@ sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t)
imu.angular_velocity.z = gyr(2);
return imu;
}
}
geometry_msgs::TwistStamped twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t)
{
geometry_msgs::TwistStamped
twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t) {
geometry_msgs::TwistStamped twist;
twist.header.stamp = toRosTime(t);
@ -117,10 +121,11 @@ geometry_msgs::TwistStamped twistToMsg(const AngularVelocity& w, const LinearVel
twist.twist.linear.z = v(2);
return twist;
}
}
void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg)
{
void cameraToMsg(
const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg
) {
CHECK(msg);
msg->width = camera->width();
msg->height = camera->height();
@ -129,16 +134,11 @@ void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoP
CalibrationMatrix K = calibrationMatrixFromCamera(camera);
boost::array<double, 9> K_vec;
std::vector<double> D_vec;
for(int i=0; i<3; ++i)
{
for(int j=0; j<3; ++j)
{
K_vec[j+i*3] = static_cast<double>(K(i,j));
}
}
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
K_vec[j + i * 3] = static_cast<double>(K(i, j));
switch(camera->type())
{
switch (camera->type()) {
case ze::CameraType::PinholeRadialTangential:
case ze::CameraType::Pinhole:
msg->distortion_model = "plumb_bob";
@ -155,20 +155,15 @@ void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoP
break;
}
for(int j=0; j<camera->distortionParameters().rows(); ++j)
{
D_vec.push_back(static_cast<double>(camera->distortionParameters()(j))); // @TODO: use the distortion params from the camera
for (int j = 0; j < camera->distortionParameters().rows(); ++j) {
D_vec.push_back(static_cast<double>(camera->distortionParameters()(j
))); // @TODO: use the distortion params from the camera
}
msg->K = K_vec;
msg->D = D_vec;
msg->P = {K(0,0), 0, K(0,2), 0,
0, K(1,1), K(1,2), 0,
0, 0, 1, 0};
msg->R = {1, 0, 0,
0, 1, 0,
0, 0, 1};
}
msg->P = {K(0, 0), 0, K(0, 2), 0, 0, K(1, 1), K(1, 2), 0, 0, 0, 1, 0};
msg->R = {1, 0, 0, 0, 1, 0, 0, 0, 1};
}
} // namespace event_camera_simulator

342
event_camera_simulator/esim_visualization/src/rosbag_writer.cpp
View File

@ -1,13 +1,12 @@
#include <esim/visualization/rosbag_writer.hpp>
#include <esim/common/utils.hpp>
#include <ze/common/time_conversions.hpp>
#include <esim/visualization/ros_utils.hpp>
#include <esim/visualization/rosbag_writer.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <minkindr_conversions/kindr_msg.h>
#include <minkindr_conversions/kindr_tf.h>
#include <tf/tfMessage.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <ze/common/time_conversions.hpp>
DECLARE_double(ros_publisher_camera_info_rate);
DECLARE_double(ros_publisher_frame_rate);
@ -15,24 +14,24 @@ DECLARE_double(ros_publisher_depth_rate);
DECLARE_double(ros_publisher_pointcloud_rate);
DECLARE_double(ros_publisher_optic_flow_rate);
DEFINE_string(path_to_output_bag, "",
"Path to which save the output bag file.");
DEFINE_string(
path_to_output_bag, "", "Path to which save the output bag file."
);
namespace event_camera_simulator {
RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, size_t num_cameras)
{
RosbagWriter::RosbagWriter(
const std::string& path_to_output_bag, size_t num_cameras
) {
CHECK_GE(num_cameras, 1);
num_cameras_ = num_cameras;
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
try
{
try {
bag_.open(path_to_output_bag, rosbag::bagmode::Write);
}
catch(rosbag::BagIOException e)
{
LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag << std::endl;
} catch (rosbag::BagIOException e) {
LOG(FATAL) << "Error: could not open rosbag: "
<< FLAGS_path_to_output_bag << std::endl;
return;
}
@ -44,182 +43,194 @@ RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, size_t num_cam
last_published_depthmap_time_ = 0;
last_published_optic_flow_time_ = 0;
last_published_pointcloud_time_ = 0;
}
}
Publisher::Ptr RosbagWriter::createBagWriterFromGflags(size_t num_cameras)
{
if(FLAGS_path_to_output_bag == "")
{
Publisher::Ptr RosbagWriter::createBagWriterFromGflags(size_t num_cameras) {
if (FLAGS_path_to_output_bag == "") {
LOG(INFO) << "Empty output bag string: will not write to rosbag";
return nullptr;
}
return std::make_shared<RosbagWriter>(FLAGS_path_to_output_bag, num_cameras);
}
return std::make_shared<RosbagWriter>(
FLAGS_path_to_output_bag,
num_cameras
);
}
RosbagWriter::~RosbagWriter()
{
RosbagWriter::~RosbagWriter() {
LOG(INFO) << "Finalizing the bag...";
bag_.close();
LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag;
}
}
void RosbagWriter::pointcloudCallback(const PointCloudVector& pointclouds, Time t)
{
void RosbagWriter::pointcloudCallback(
const PointCloudVector& pointclouds, Time t
) {
CHECK_EQ(pointclouds.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
Duration min_time_interval_between_published_pointclouds_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
if(last_published_pointcloud_time_ > 0 && t - last_published_pointcloud_time_ < min_time_interval_between_published_pointclouds_)
{
for (size_t i = 0; i < num_cameras_; ++i) {
Duration min_time_interval_between_published_pointclouds_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
if (last_published_pointcloud_time_ > 0
&& t - last_published_pointcloud_time_
< min_time_interval_between_published_pointclouds_) {
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGB>);
std::stringstream ss; ss << "cam" << i;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
msg(new pcl::PointCloud<pcl::PointXYZRGB>);
std::stringstream ss;
ss << "cam" << i;
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "pointcloud"),
toRosTime(t), msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "pointcloud"),
toRosTime(t),
msg
);
}
last_published_pointcloud_time_ = t;
}
}
void RosbagWriter::imageCallback(const ImagePtrVector& images, Time t)
{
for(size_t i=0; i<num_cameras_; ++i)
{
void RosbagWriter::imageCallback(const ImagePtrVector& images, Time t) {
for (size_t i = 0; i < num_cameras_; ++i) {
sensor_sizes_[i] = images[i]->size();
static const Duration min_time_interval_between_published_images_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if(last_published_image_time_ > 0 && t - last_published_image_time_ < min_time_interval_between_published_images_)
{
static const Duration min_time_interval_between_published_images_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if (last_published_image_time_ > 0
&& t - last_published_image_time_
< min_time_interval_between_published_images_) {
return;
}
if(images[i])
{
if (images[i]) {
sensor_msgs::ImagePtr msg;
imageToMsg(*images[i], t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "image_raw"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "image_raw"),
msg->header.stamp,
msg
);
}
}
last_published_image_time_ = t;
}
}
void RosbagWriter::imageCorruptedCallback(const ImagePtrVector& images_corrupted, Time t)
{
for(size_t i=0; i<num_cameras_; ++i)
{
static const Duration min_time_interval_between_published_images_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if(last_published_corrupted_image_time_ > 0 && t - last_published_corrupted_image_time_ < min_time_interval_between_published_images_)
{
void RosbagWriter::imageCorruptedCallback(
const ImagePtrVector& images_corrupted, Time t
) {
for (size_t i = 0; i < num_cameras_; ++i) {
static const Duration min_time_interval_between_published_images_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
if (last_published_corrupted_image_time_ > 0
&& t - last_published_corrupted_image_time_
< min_time_interval_between_published_images_) {
return;
}
if(images_corrupted[i])
{
if (images_corrupted[i]) {
sensor_msgs::ImagePtr msg;
imageToMsg(*images_corrupted[i], t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "image_corrupted"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "image_corrupted"),
msg->header.stamp,
msg
);
}
}
last_published_corrupted_image_time_ = t;
}
}
void
RosbagWriter::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {
if (depthmaps.size() != num_cameras_)
return;
void RosbagWriter::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t)
{
if(depthmaps.size() != num_cameras_)
{
for (size_t i = 0; i < num_cameras_; ++i) {
static const Duration
min_time_interval_between_published_depthmaps_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
if (last_published_depthmap_time_ > 0
&& t - last_published_depthmap_time_
< min_time_interval_between_published_depthmaps_) {
return;
}
for(size_t i=0; i<num_cameras_; ++i)
{
static const Duration min_time_interval_between_published_depthmaps_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
if(last_published_depthmap_time_ > 0 && t - last_published_depthmap_time_ < min_time_interval_between_published_depthmaps_)
{
return;
}
if(depthmaps[i])
{
if (depthmaps[i]) {
sensor_msgs::ImagePtr msg;
depthmapToMsg(*depthmaps[i], t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "depthmap"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "depthmap"),
msg->header.stamp,
msg
);
}
}
last_published_depthmap_time_ = t;
}
}
void RosbagWriter::opticFlowCallback(
const OpticFlowPtrVector& optic_flows, Time t
) {
if (optic_flows.size() != num_cameras_)
return;
void RosbagWriter::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
{
if(optic_flows.size() != num_cameras_)
{
for (size_t i = 0; i < num_cameras_; ++i) {
static const Duration
min_time_interval_between_published_optic_flows_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate);
if (last_published_optic_flow_time_ > 0
&& t - last_published_optic_flow_time_
< min_time_interval_between_published_optic_flows_) {
return;
}
for(size_t i=0; i<num_cameras_; ++i)
{
static const Duration min_time_interval_between_published_optic_flows_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate);
if(last_published_optic_flow_time_ > 0 && t - last_published_optic_flow_time_ < min_time_interval_between_published_optic_flows_)
{
return;
}
if(optic_flows[i])
{
if (optic_flows[i]) {
esim_msgs::OpticFlow::Ptr msg;
msg.reset(new esim_msgs::OpticFlow);
opticFlowToMsg(*optic_flows[i], t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "optic_flow"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "optic_flow"),
msg->header.stamp,
msg
);
}
}
last_published_optic_flow_time_ = t;
}
void RosbagWriter::eventsCallback(const EventsVector& events)
{
for(size_t i=0; i<num_cameras_; ++i)
{
if(sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
{
continue;
}
if(events[i].empty())
{
void RosbagWriter::eventsCallback(const EventsVector& events) {
for (size_t i = 0; i < num_cameras_; ++i) {
if (sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
continue;
if (events[i].empty())
continue;
}
dvs_msgs::EventArrayPtr msg;
msg.reset(new dvs_msgs::EventArray);
eventsToMsg(events[i], sensor_sizes_[i].width, sensor_sizes_[i].height, msg);
eventsToMsg(
events[i],
sensor_sizes_[i].width,
sensor_sizes_[i].height,
msg
);
bag_.write(getTopicName(topic_name_prefix_, i, "events"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "events"),
msg->header.stamp,
msg
);
}
}
}
void RosbagWriter::poseCallback(const Transformation& T_W_B,
const TransformationVector& T_W_Cs,
Time t)
{
if(T_W_Cs.size() != num_cameras_)
{
LOG(WARNING) << "Number of poses is different than number of cameras."
void RosbagWriter::poseCallback(
const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t
) {
if (T_W_Cs.size() != num_cameras_) {
LOG(WARNING
) << "Number of poses is different than number of cameras."
<< "Will not output poses.";
return;
}
@ -229,17 +240,28 @@ void RosbagWriter::poseCallback(const Transformation& T_W_B,
transform_stamped_msg.header.stamp = toRosTime(t);
tf::tfMessage tf_msg;
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
// Write pose to bag
tf::poseStampedKindrToMsg(T_W_Cs[i], toRosTime(t), "map", &pose_stamped_msg);
bag_.write(getTopicName(topic_name_prefix_, i, "pose"),
toRosTime(t), pose_stamped_msg);
tf::poseStampedKindrToMsg(
T_W_Cs[i],
toRosTime(t),
"map",
&pose_stamped_msg
);
bag_.write(
getTopicName(topic_name_prefix_, i, "pose"),
toRosTime(t),
pose_stamped_msg
);
// Write tf transform to bag
std::stringstream ss; ss << "cam" << i;
std::stringstream ss;
ss << "cam" << i;
transform_stamped_msg.child_frame_id = ss.str();
tf::transformKindrToMsg(T_W_Cs[i], &transform_stamped_msg.transform);
tf::transformKindrToMsg(
T_W_Cs[i],
&transform_stamped_msg.transform
);
tf_msg.transforms.push_back(transform_stamped_msg);
}
@ -248,61 +270,65 @@ void RosbagWriter::poseCallback(const Transformation& T_W_B,
tf_msg.transforms.push_back(transform_stamped_msg);
bag_.write("/tf", toRosTime(t), tf_msg);
}
}
void RosbagWriter::twistCallback(const AngularVelocityVector &ws, const LinearVelocityVector &vs, Time t)
{
if(ws.size() != num_cameras_
|| vs.size() != num_cameras_)
{
LOG(WARNING) << "Number of twists is different than number of cameras."
void RosbagWriter::twistCallback(
const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t
) {
if (ws.size() != num_cameras_ || vs.size() != num_cameras_) {
LOG(WARNING
) << "Number of twists is different than number of cameras."
<< "Will not output twists.";
return;
}
CHECK_EQ(ws.size(), num_cameras_);
CHECK_EQ(vs.size(), num_cameras_);
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
bag_.write(getTopicName(topic_name_prefix_, i, "twist"),
msg.header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "twist"),
msg.header.stamp,
msg
);
}
}
}
void RosbagWriter::imuCallback(const Vector3& acc, const Vector3& gyr, Time t)
{
void
RosbagWriter::imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {
VLOG_EVERY_N(1, 500) << "t = " << ze::nanosecToSecTrunc(t) << " s";
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
const std::string imu_topic = "/imu";
bag_.write(imu_topic,
msg.header.stamp, msg);
}
bag_.write(imu_topic, msg.header.stamp, msg);
}
void RosbagWriter::cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t)
{
void RosbagWriter::cameraInfoCallback(
const ze::CameraRig::Ptr& camera_rig, Time t
) {
CHECK(camera_rig);
CHECK_EQ(camera_rig->size(), num_cameras_);
static const Duration min_time_interval_between_published_camera_info_
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
if(last_published_camera_info_time_ > 0 && t - last_published_camera_info_time_ < min_time_interval_between_published_camera_info_)
{
static const Duration min_time_interval_between_published_camera_info_ =
ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
if (last_published_camera_info_time_ > 0
&& t - last_published_camera_info_time_
< min_time_interval_between_published_camera_info_) {
return;
}
for(size_t i=0; i<num_cameras_; ++i)
{
for (size_t i = 0; i < num_cameras_; ++i) {
sensor_msgs::CameraInfoPtr msg;
msg.reset(new sensor_msgs::CameraInfo);
cameraToMsg(camera_rig->atShared(i), t, msg);
bag_.write(getTopicName(topic_name_prefix_, i, "camera_info"),
msg->header.stamp, msg);
bag_.write(
getTopicName(topic_name_prefix_, i, "camera_info"),
msg->header.stamp,
msg
);
}
last_published_camera_info_time_ = t;
}
}
} // namespace event_camera_simulator

132
event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp
View File

@ -1,52 +1,60 @@
#include <esim/visualization/synthetic_optic_flow_publisher.hpp>
#include <esim/common/utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/time_conversions.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <esim/visualization/synthetic_optic_flow_publisher.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui/highgui.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/path_utils.hpp>
#include <ze/common/time_conversions.hpp>
DEFINE_string(synthetic_optic_flow_output_folder, "",
"Folder in which to output the events.");
DEFINE_string(
synthetic_optic_flow_output_folder,
"",
"Folder in which to output the events."
);
namespace event_camera_simulator {
/**
* This publisher was designed with the purpose of generating simulation data
* with ground truth labels, for the task of optic flow estimation.
/**
* This publisher was designed with the purpose of generating simulation
* data with ground truth labels, for the task of optic flow estimation.
*
* It assumes that it will receive a relatively small sequence of events (corresponding, for example,
* to all the events in between two frames), and will write all the events to disk in its destructor,
* in three forms:
* - an "events.txt" file that contains all the events in "t x y pol" format (one event per line)
* - an "event_count.png" image that whose first two channels contain the counts of the positive (resp. negative) event counts at each pixel
* - two "timestamps images" in which each pixel contains the timestamp at the last event that fell on the pixel.
* (since the timestamp is a floating point value, it is split in 3 8-bit values so that the timestamp images
* It assumes that it will receive a relatively small sequence of events
* (corresponding, for example, to all the events in between two frames),
* and will write all the events to disk in its destructor, in three forms:
* - an "events.txt" file that contains all the events in "t x y pol"
* format (one event per line)
* - an "event_count.png" image that whose first two channels contain the
* counts of the positive (resp. negative) event counts at each pixel
* - two "timestamps images" in which each pixel contains the timestamp at
* the last event that fell on the pixel. (since the timestamp is a floating
* point value, it is split in 3 8-bit values so that the timestamp images
* can be saved in a single 3-channel image).
*/
SyntheticOpticFlowPublisher::SyntheticOpticFlowPublisher(const std::string& output_folder)
: output_folder_(output_folder)
{
ze::openOutputFileStream(ze::joinPath(output_folder, "events.txt"),
&events_file_);
}
SyntheticOpticFlowPublisher::SyntheticOpticFlowPublisher(
const std::string& output_folder
)
: output_folder_(output_folder) {
ze::openOutputFileStream(
ze::joinPath(output_folder, "events.txt"),
&events_file_
);
}
Publisher::Ptr SyntheticOpticFlowPublisher::createFromGflags()
{
if(FLAGS_synthetic_optic_flow_output_folder == "")
{
LOG(WARNING) << "Empty output folder string: will not write synthetic optic flow files";
Publisher::Ptr SyntheticOpticFlowPublisher::createFromGflags() {
if (FLAGS_synthetic_optic_flow_output_folder == "") {
LOG(WARNING
) << "Empty output folder string: will not write synthetic "
"optic flow files";
return nullptr;
}
return std::make_shared<SyntheticOpticFlowPublisher>(FLAGS_synthetic_optic_flow_output_folder);
}
return std::make_shared<SyntheticOpticFlowPublisher>(
FLAGS_synthetic_optic_flow_output_folder
);
}
SyntheticOpticFlowPublisher::~SyntheticOpticFlowPublisher()
{
SyntheticOpticFlowPublisher::~SyntheticOpticFlowPublisher() {
// Create an event count image using all the events collected
cv::Mat event_count_image = cv::Mat::zeros(sensor_size_, CV_8UC3);
@ -56,52 +64,64 @@ SyntheticOpticFlowPublisher::~SyntheticOpticFlowPublisher()
int remapped_timestamp_fraction;
double timestamp_fraction;
for(Event e : events_)
{
event_count_image.at<cv::Vec3b>(e.y,e.x)[int(e.pol)]++;
for (Event e : events_) {
event_count_image.at<cv::Vec3b>(e.y, e.x)[int(e.pol)]++;
cv::Mat& curr_timestamp_image = e.pol ? timestamps_pos : timestamps_neg;
cv::Mat& curr_timestamp_image =
e.pol ? timestamps_pos : timestamps_neg;
// remap value
timestamp_fraction = double(e.t - events_[0].t) / (events_[events_.size()-1].t - events_[0].t);
remapped_timestamp_fraction = timestamp_fraction * std::pow(2,24); // remap 0-1 to 0 - 2^24
timestamp_fraction = double(e.t - events_[0].t)
/ (events_[events_.size() - 1].t - events_[0].t);
remapped_timestamp_fraction =
timestamp_fraction * std::pow(2, 24); // remap 0-1 to 0 - 2^24
// distribute the 24 bit number (remapped_timestamp_fraction) to 3 channel 8 bit image
for (int i=0; i<3; i++)
{
curr_timestamp_image.at<cv::Vec3b>(e.y,e.x)[i] = (int) remapped_timestamp_fraction & 0xFF; // bit mask of 0000 0000 0000 0000 1111 1111
remapped_timestamp_fraction = remapped_timestamp_fraction >> 8; // shifts bits to right by 8
// distribute the 24 bit number (remapped_timestamp_fraction) to 3
// channel 8 bit image
for (int i = 0; i < 3; i++) {
curr_timestamp_image.at<cv::Vec3b>(e.y, e.x)[i] =
(int) remapped_timestamp_fraction
& 0xFF; // bit mask of 0000 0000 0000 0000 1111 1111
remapped_timestamp_fraction = remapped_timestamp_fraction
>> 8; // shifts bits to right by 8
}
}
// Write event count image + the two timestamps images to disk
std::string path_event_count_image = ze::joinPath(output_folder_, "event_count.png");
std::string path_timestamps_pos = ze::joinPath(output_folder_, "event_time_stamps_pos.png");
std::string path_timestamps_neg = ze::joinPath(output_folder_, "event_time_stamps_neg.png");
std::string path_event_count_image =
ze::joinPath(output_folder_, "event_count.png");
std::string path_timestamps_pos =
ze::joinPath(output_folder_, "event_time_stamps_pos.png");
std::string path_timestamps_neg =
ze::joinPath(output_folder_, "event_time_stamps_neg.png");
std::vector<int> compression_params;
compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
compression_params.push_back(0);
cv::imwrite(path_event_count_image, event_count_image, compression_params);
cv::imwrite(
path_event_count_image,
event_count_image,
compression_params
);
cv::imwrite(path_timestamps_pos, timestamps_pos, compression_params);
cv::imwrite(path_timestamps_neg, timestamps_neg, compression_params);
// Finish writing event file
events_file_.close();
}
}
void SyntheticOpticFlowPublisher::eventsCallback(const EventsVector& events)
{
void SyntheticOpticFlowPublisher::eventsCallback(const EventsVector& events
) {
CHECK_EQ(events.size(), 1);
// Simply aggregate the events into the events_ buffer.
// At the destruction of this object, everything will be saved to disk.
for(const Event& e : events[0])
{
events_file_ << e.t << " " << e.x << " " << e.y << " " << (e.pol? 1 : 0) << std::endl;
for (const Event& e : events[0]) {
events_file_ << e.t << " " << e.x << " " << e.y << " "
<< (e.pol ? 1 : 0) << std::endl;
events_.push_back(e);
}
}
}
} // namespace event_camera_simulator

28
event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp
View File

@ -1,35 +1,39 @@
#pragma once
#include <esim/rendering/simple_renderer_base.hpp>
#include <esim/imp_multi_objects_2d/object.hpp>
#include <esim/rendering/simple_renderer_base.hpp>
namespace event_camera_simulator {
//! A rendering engine for planar scenes
class MultiObject2DRenderer : public SimpleRenderer
{
public:
//! A rendering engine for planar scenes
class MultiObject2DRenderer : public SimpleRenderer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MultiObject2DRenderer();
~MultiObject2DRenderer();
virtual bool render(const Time t,
virtual bool render(
const Time t,
const ImagePtr& out_image,
const OpticFlowPtr& optic_flow_map) const;
const OpticFlowPtr& optic_flow_map
) const;
virtual int getWidth() const { return width_; }
virtual int getWidth() const {
return width_;
}
virtual int getHeight() const { return height_; }
protected:
virtual int getHeight() const {
return height_;
}
protected:
void outputGroundTruthData();
std::vector<std::shared_ptr<Object>> objects_;
int width_, height_;
ze::real_t tmax_;
};
};
} // namespace event_camera_simulator

124
event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/object.hpp
View File

@ -4,21 +4,31 @@
namespace event_camera_simulator {
bool loadTexture(const std::string& path_to_img, cv::Mat *img,
double median_blur, double gaussian_blur);
bool loadTexture(
const std::string& path_to_img,
cv::Mat* img,
double median_blur,
double gaussian_blur
);
using Affine = cv::Matx<FloatType, 3,3>;
using AffineWithJacobian = std::pair<Affine, Affine>;
using Affine = cv::Matx<FloatType, 3, 3>;
using AffineWithJacobian = std::pair<Affine, Affine>;
class MotionParameters
{
public:
MotionParameters(FloatType tmax,
FloatType theta0_deg, FloatType theta1_deg,
FloatType x0, FloatType x1,
FloatType y0, FloatType y1,
FloatType sx0, FloatType sx1,
FloatType sy0, FloatType sy1)
class MotionParameters {
public:
MotionParameters(
FloatType tmax,
FloatType theta0_deg,
FloatType theta1_deg,
FloatType x0,
FloatType x1,
FloatType y0,
FloatType y1,
FloatType sx0,
FloatType sx1,
FloatType sy0,
FloatType sy1
)
: tmax_(tmax),
x0_(x0),
x1_(x1),
@ -29,12 +39,9 @@ public:
sx0_(sx0),
sx1_(sx1),
sy0_(sy0),
sy1_(sy1)
{
}
sy1_(sy1) {}
AffineWithJacobian getAffineTransformationWithJacobian(ze::real_t t)
{
AffineWithJacobian getAffineTransformationWithJacobian(ze::real_t t) {
// constants
const ze::real_t dtheta = theta1_ - theta0_;
const ze::real_t dx = x1_ - x0_;
@ -43,30 +50,30 @@ public:
const ze::real_t dsy = sy1_ - sy0_;
// computation of parameter(t)
const ze::real_t theta = theta0_ + t/tmax_ * dtheta;
const ze::real_t x = x0_ + t/tmax_ * dx;
const ze::real_t y = y0_ + t/tmax_ * dy;
const ze::real_t sx = sx0_ + t/tmax_ * dsx;
const ze::real_t sy = sy0_ + t/tmax_ * dsy;
const ze::real_t theta = theta0_ + t / tmax_ * dtheta;
const ze::real_t x = x0_ + t / tmax_ * dx;
const ze::real_t y = y0_ + t / tmax_ * dy;
const ze::real_t sx = sx0_ + t / tmax_ * dsx;
const ze::real_t sy = sy0_ + t / tmax_ * dsy;
const ze::real_t stheta = std::sin(theta);
const ze::real_t ctheta = std::cos(theta);
Affine A;
A << sx * ctheta, -sy * stheta, x,
sx * stheta, sy * ctheta, y,
0, 0, 1;
A << sx * ctheta, -sy * stheta, x, sx * stheta, sy * ctheta, y, 0,
0, 1;
// computation of dparameter_dt(t)
const ze::real_t dtheta_dt = 1./tmax_ * dtheta;
const ze::real_t dx_dt = 1./tmax_ * dx;
const ze::real_t dy_dt = 1./tmax_ * dy;
const ze::real_t dsx_dt = 1./tmax_ * dsx;
const ze::real_t dsy_dt = 1./tmax_ * dsy;
const ze::real_t dtheta_dt = 1. / tmax_ * dtheta;
const ze::real_t dx_dt = 1. / tmax_ * dx;
const ze::real_t dy_dt = 1. / tmax_ * dy;
const ze::real_t dsx_dt = 1. / tmax_ * dsx;
const ze::real_t dsy_dt = 1. / tmax_ * dsy;
cv::Matx<FloatType, 3, 3> dAdt;
dAdt << dsx_dt * ctheta - dtheta_dt * stheta * sx, -dsy_dt * stheta - dtheta_dt * ctheta * sy, dx_dt,
dsx_dt * stheta + dtheta_dt * ctheta * sx, dsy_dt * ctheta - dtheta_dt * stheta * sy, dy_dt,
0.0, 0.0, 0.0;
dAdt << dsx_dt * ctheta - dtheta_dt * stheta * sx,
-dsy_dt * stheta - dtheta_dt * ctheta * sy, dx_dt,
dsx_dt * stheta + dtheta_dt * ctheta * sx,
dsy_dt * ctheta - dtheta_dt * stheta * sy, dy_dt, 0.0, 0.0, 0.0;
return AffineWithJacobian(A, dAdt);
}
@ -77,45 +84,56 @@ public:
FloatType theta0_, theta1_;
FloatType sx0_, sx1_;
FloatType sy0_, sy1_;
};
};
class Object
{
public:
Object(const std::string path_to_texture, const cv::Size& dst_size, const MotionParameters& motion_params,
double median_blur, double gaussian_blur);
class Object {
public:
Object(
const std::string path_to_texture,
const cv::Size& dst_size,
const MotionParameters& motion_params,
double median_blur,
double gaussian_blur
);
void draw(Time t, bool is_first_layer = false);
cv::Mat canvas_;
OpticFlow flow_;
MotionParameters getMotionParameters() const { return p_; }
MotionParameters getMotionParameters() const {
return p_;
}
Affine getK0() const { return K0_; }
Affine getK1() const { return K1_; }
Affine getK0() const {
return K0_;
}
private:
Affine getK1() const {
return K1_;
}
private:
cv::Mat texture_;
cv::Size dst_size_;
MotionParameters p_;
Affine K0_, K1_;
};
};
void getIntensityAndAlpha(const cv::Mat& image,
int x, int y,
void getIntensityAndAlpha(
const cv::Mat& image,
int x,
int y,
ImageFloatType* intensity,
ImageFloatType* alpha);
ImageFloatType* alpha
);
inline ImageFloatType bgrToGrayscale(uchar b,
uchar g,
uchar r)
{
inline ImageFloatType bgrToGrayscale(uchar b, uchar g, uchar r) {
// https://www.johndcook.com/blog/2009/08/24/algorithms-convert-color-grayscale/
return 0.07 * static_cast<ImageFloatType>(b)
+ 0.72 * static_cast<ImageFloatType>(g)
+ 0.21 * static_cast<ImageFloatType>(r);
}
}
} // namespace event_camera_simulator

313
event_camera_simulator/imp/imp_multi_objects_2d/src/imp_multi_objects_2d_renderer.cpp
View File

@ -1,11 +1,10 @@
#include <esim/common/utils.hpp>
#include <esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/common/file_utils.hpp>
#include <ze/common/time_conversions.hpp>
@ -13,19 +12,29 @@ DECLARE_double(renderer_preprocess_median_blur);
DECLARE_double(renderer_preprocess_gaussian_blur);
DEFINE_string(path_to_sequence_file, "",
"Path to the sequence file that describes the 2D, multi-object scene.");
DEFINE_string(
path_to_sequence_file,
"",
"Path to the sequence file that describes the 2D, multi-object scene."
);
DEFINE_string(path_to_output_folder, "",
"Path to the output folder where the flow ground truth files will be written");
DEFINE_string(
path_to_output_folder,
"",
"Path to the output folder where the flow ground truth files "
"will be written"
);
DEFINE_bool(output_reverse_displacement_map, false,
"Whether to output the reverse displacement map (i.e., the displacement field that maps pixels at t=tmax to pixels at t=t0");
DEFINE_bool(
output_reverse_displacement_map,
false,
"Whether to output the reverse displacement map (i.e., the "
"displacement field that maps pixels at t=tmax to pixels at t=t0"
);
namespace event_camera_simulator {
MultiObject2DRenderer::MultiObject2DRenderer()
{
MultiObject2DRenderer::MultiObject2DRenderer() {
// Load sequence from file
const std::string path_to_sequence_file = FLAGS_path_to_sequence_file;
@ -33,7 +42,8 @@ MultiObject2DRenderer::MultiObject2DRenderer()
ze::openFileStream(path_to_sequence_file, &sequence_file);
std::string path_to_texture;
double median_blur, gaussian_blur, theta0, theta1, x0, x1, y0, y1, sx0, sx1, sy0, sy1;
double median_blur, gaussian_blur, theta0, theta1, x0, x1, y0, y1, sx0,
sx1, sy0, sy1;
sequence_file >> width_ >> height_ >> tmax_;
@ -44,47 +54,45 @@ MultiObject2DRenderer::MultiObject2DRenderer()
VLOG(1) << "width = " << width_ << ", height = " << height_;
VLOG(1) << "tmax = " << tmax_;
while(sequence_file >> path_to_texture
>> median_blur >> gaussian_blur
>> theta0 >> theta1
>> x0 >> x1
>> y0 >> y1
>> sx0 >> sx1
>> sy0 >> sy1)
{
MotionParameters params(tmax_,
theta0, theta1,
x0, x1,
y0, y1,
sx0, sx1,
sy0, sy1);
while (sequence_file >> path_to_texture >> median_blur >> gaussian_blur
>> theta0 >> theta1 >> x0 >> x1 >> y0 >> y1 >> sx0 >> sx1 >> sy0
>> sy1) {
MotionParameters params(
tmax_,
theta0,
theta1,
x0,
x1,
y0,
y1,
sx0,
sx1,
sy0,
sy1
);
objects_.emplace_back(std::make_shared<Object>(path_to_texture,
objects_.emplace_back(std::make_shared<Object>(
path_to_texture,
cv::Size(width_, height_),
params,
median_blur,
gaussian_blur));
gaussian_blur
));
}
if(FLAGS_path_to_output_folder != "")
{
if (FLAGS_path_to_output_folder != "")
outputGroundTruthData();
}
}
MultiObject2DRenderer::~MultiObject2DRenderer()
{
}
MultiObject2DRenderer::~MultiObject2DRenderer() {}
bool MultiObject2DRenderer::render(const Time t,
bool MultiObject2DRenderer::render(
const Time t,
const ImagePtr& out_image,
const OpticFlowPtr& optic_flow_map) const {
if(ze::nanosecToSecTrunc(t) > tmax_)
{
const OpticFlowPtr& optic_flow_map
) const {
if (ze::nanosecToSecTrunc(t) > tmax_)
return true;
}
CHECK_EQ(out_image->rows, height_);
CHECK_EQ(out_image->cols, width_);
@ -92,8 +100,7 @@ bool MultiObject2DRenderer::render(const Time t,
out_image->setTo(0);
optic_flow_map->setTo(0.);
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
const bool is_first_layer = (i == 0);
objects_[i]->draw(t, is_first_layer);
}
@ -105,176 +112,212 @@ bool MultiObject2DRenderer::render(const Time t,
ImageFloatType intensity;
ImageFloatType alpha;
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
const std::shared_ptr<Object>& object = objects_[i];
const cv::Mat& image = object->canvas_;
const OpticFlow& flow = object->flow_;
for(int y=0; y<out_image->rows; ++y)
{
for(int x=0; x<out_image->cols; ++x)
{
for (int y = 0; y < out_image->rows; ++y) {
for (int x = 0; x < out_image->cols; ++x) {
getIntensityAndAlpha(image, x, y, &intensity, &alpha);
(*out_image)(y,x) = alpha * intensity + (1.-alpha) * (*out_image)(y,x);
(*out_image)(y, x) =
alpha * intensity + (1. - alpha) * (*out_image)(y, x);
if(alpha > 0)
{
(*optic_flow_map)(y,x) = flow(y,x);
}
if (alpha > 0)
(*optic_flow_map)(y, x) = flow(y, x);
}
}
}
return false;
}
}
void MultiObject2DRenderer::outputGroundTruthData()
{
// This function generates some ground truth information and output if to the FLAGS_path_to_output_folder
// In principle, this information could be transmitted in a SimulationData structure and forwarded
// to a Publisher object that writes it to disk.
// However, for technical reasons, it is more convenient to write the displacement maps here, rather than integrate instantaneous
// optic flow maps in a Publisher.
void MultiObject2DRenderer::outputGroundTruthData() {
// This function generates some ground truth information and output if
// to the FLAGS_path_to_output_folder In principle, this information
// could be transmitted in a SimulationData structure and forwarded to a
// Publisher object that writes it to disk. However, for technical
// reasons, it is more convenient to write the displacement maps here,
// rather than integrate instantaneous optic flow maps in a Publisher.
// The following ground truth information is computed below and output to the ground truth folder:
// The following ground truth information is computed below and output
// to the ground truth folder:
// - Displacement maps from t=0 to t=tmax (and vice-versa)
// - Images 0 and images 1
VLOG(1) << "Will output ground truth data to folder: " << FLAGS_path_to_output_folder;
VLOG(1) << "Will output ground truth data to folder: "
<< FLAGS_path_to_output_folder;
// Notation: the frame at time t=0 is denoted as frame 0, and the frame at time t=tmax is denoted as frame 1
// Compute the entire displacement field from 0 to 1, for every layer
// Notation: the frame at time t=0 is denoted as frame 0, and the frame
// at time t=tmax is denoted as frame 1 Compute the entire displacement
// field from 0 to 1, for every layer
const ze::real_t t0 = 0.0;
const ze::real_t t1 = tmax_;
// Every entry in this vector is a displacement map that maps pixel locations in image 0
// to the corresponding pixel location in image 1, for the layer i
// Every entry in this vector is a displacement map that maps pixel
// locations in image 0 to the corresponding pixel location in image 1,
// for the layer i
//
// this individual displacement layers are then merged together to form the final
// displacement map later
// this individual displacement layers are then merged together to form
// the final displacement map later
//
// Note: the reverse layer-wise displacement map can be computed as follows:
// Note: the reverse layer-wise displacement map can be computed as
// follows:
// layer_displacement_01[i] = -layer_displacement_10[i]
std::vector<OpticFlow> layer_displacements_10(objects_.size());
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
layer_displacements_10[i] = OpticFlow(height_, width_);
const Object object = *(objects_[i]);
Affine A_t0_src = object.getMotionParameters().getAffineTransformationWithJacobian(t0).first;
Affine A_t1_src = object.getMotionParameters().getAffineTransformationWithJacobian(t1).first;
Affine A_t0_src = object.getMotionParameters()
.getAffineTransformationWithJacobian(t0)
.first;
Affine A_t1_src = object.getMotionParameters()
.getAffineTransformationWithJacobian(t1)
.first;
Affine A_t1_t0 = A_t1_src * A_t0_src.inv();
// M_t1_t0 maps any point on the first image to its position in the last image
// M_t1_t0 maps any point on the first image to its position in the
// last image
Affine M_t1_t0 = object.getK1() * A_t1_t0 * object.getK1().inv();
for(int y=0; y<height_; ++y)
{
for(int x=0; x<width_; ++x)
{
FloatType x_t1 = M_t1_t0(0,0) * x + M_t1_t0(0,1) * y + M_t1_t0(0,2);
FloatType y_t1 = M_t1_t0(1,0) * x + M_t1_t0(1,1) * y + M_t1_t0(1,2);
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
FloatType x_t1 =
M_t1_t0(0, 0) * x + M_t1_t0(0, 1) * y + M_t1_t0(0, 2);
FloatType y_t1 =
M_t1_t0(1, 0) * x + M_t1_t0(1, 1) * y + M_t1_t0(1, 2);
FloatType displacement_x, displacement_y;
// if the pixel went out of the field of view, we assign a displacement of 0 (convention)
displacement_x = (x_t1 >= 0 && x_t1 < width_) ? (ImageFloatType) x_t1 - (ImageFloatType) x : 0.;
displacement_y = (y_t1 >= 0 && y_t1 < height_) ? (ImageFloatType) y_t1 - (ImageFloatType) y : 0.;
// if the pixel went out of the field of view, we assign a
// displacement of 0 (convention)
displacement_x =
(x_t1 >= 0 && x_t1 < width_)
? (ImageFloatType) x_t1 - (ImageFloatType) x
: 0.;
displacement_y =
(y_t1 >= 0 && y_t1 < height_)
? (ImageFloatType) y_t1 - (ImageFloatType) y
: 0.;
layer_displacements_10[i](y,x)[0] = displacement_x;
layer_displacements_10[i](y,x)[1] = displacement_y;
layer_displacements_10[i](y, x)[0] = displacement_x;
layer_displacements_10[i](y, x)[1] = displacement_y;
}
}
}
ImageFloatType intensity, alpha;
// First, merge the displacement map from 0 to 1
OpticFlow displacement_map_10(height_, width_); // displacement map from 0 to 1
OpticFlow displacement_map_10(
height_,
width_
); // displacement map from 0 to 1
Image image0(height_, width_);
image0.setTo(0);
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
const bool is_first_layer = (i == 0);
objects_[i]->draw(t0, is_first_layer);
}
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
const std::shared_ptr<Object>& object = objects_[i];
for(int y=0; y<height_; ++y)
{
for(int x=0; x<width_; ++x)
{
getIntensityAndAlpha(object->canvas_, x, y, &intensity, &alpha);
image0(y,x) = alpha * intensity + (1.-alpha) * image0(y,x);
if(alpha > 0)
{
displacement_map_10(y,x) = layer_displacements_10[i](y,x);
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
getIntensityAndAlpha(
object->canvas_,
x,
y,
&intensity,
&alpha
);
image0(y, x) =
alpha * intensity + (1. - alpha) * image0(y, x);
if (alpha > 0) {
displacement_map_10(y, x) =
layer_displacements_10[i](y, x);
}
}
}
}
// Second, create displacement map from 1 to 0
OpticFlow displacement_map_01(height_, width_); // displacement map from 1 to 0
OpticFlow displacement_map_01(
height_,
width_
); // displacement map from 1 to 0
Image image1(height_, width_);
image1.setTo(0);
for(int i=0; i<objects_.size(); ++i)
{
for (int i = 0; i < objects_.size(); ++i) {
const bool is_first_layer = (i == 0);
objects_[i]->draw(ze::secToNanosec(t1), is_first_layer);
}
for(int i=0; i<objects_.size(); ++i)
{
for(int y=0; y<height_; ++y)
{
for(int x=0; x<width_; ++x)
{
getIntensityAndAlpha(objects_[i]->canvas_, x, y, &intensity, &alpha);
image1(y,x) = alpha * intensity + (1.-alpha) * image1(y,x);
if(alpha > 0)
{
displacement_map_01(y,x) = -layer_displacements_10[i](y,x);
for (int i = 0; i < objects_.size(); ++i) {
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
getIntensityAndAlpha(
objects_[i]->canvas_,
x,
y,
&intensity,
&alpha
);
image1(y, x) =
alpha * intensity + (1. - alpha) * image1(y, x);
if (alpha > 0) {
displacement_map_01(y, x) =
-layer_displacements_10[i](y, x);
}
}
}
}
std::ofstream displacement_file_10, displacement_file_01;
ze::openOutputFileStream(ze::joinPath(FLAGS_path_to_output_folder, "displacement_10.txt"), &displacement_file_10);
ze::openOutputFileStream(
ze::joinPath(FLAGS_path_to_output_folder, "displacement_10.txt"),
&displacement_file_10
);
if(FLAGS_output_reverse_displacement_map)
{
ze::openOutputFileStream(ze::joinPath(FLAGS_path_to_output_folder, "displacement_01.txt"), &displacement_file_01);
if (FLAGS_output_reverse_displacement_map) {
ze::openOutputFileStream(
ze::joinPath(
FLAGS_path_to_output_folder,
"displacement_01.txt"
),
&displacement_file_01
);
}
for(int y=0; y<height_; ++y)
{
for(int x=0; x<width_; ++x)
{
displacement_file_10 << displacement_map_10(y,x)[0] << " " << displacement_map_10(y,x)[1] << std::endl;
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
displacement_file_10 << displacement_map_10(y, x)[0] << " "
<< displacement_map_10(y, x)[1]
<< std::endl;
if(FLAGS_output_reverse_displacement_map)
{
displacement_file_01 << displacement_map_01(y,x)[0] << " " << displacement_map_01(y,x)[1] << std::endl;
if (FLAGS_output_reverse_displacement_map) {
displacement_file_01 << displacement_map_01(y, x)[0] << " "
<< displacement_map_01(y, x)[1]
<< std::endl;
}
}
}
displacement_file_10.close();
if(FLAGS_output_reverse_displacement_map)
{
if (FLAGS_output_reverse_displacement_map)
displacement_file_01.close();
}
cv::Mat disp_image0, disp_image1;
image0.convertTo(disp_image0, CV_8U, 255);
image1.convertTo(disp_image1, CV_8U, 255);
cv::imwrite(ze::joinPath(FLAGS_path_to_output_folder, "image0.png"), disp_image0);
cv::imwrite(ze::joinPath(FLAGS_path_to_output_folder, "image1.png"), disp_image1);
}
cv::imwrite(
ze::joinPath(FLAGS_path_to_output_folder, "image0.png"),
disp_image0
);
cv::imwrite(
ze::joinPath(FLAGS_path_to_output_folder, "image1.png"),
disp_image1
);
}
} // namespace event_camera_simulator

122
event_camera_simulator/imp/imp_multi_objects_2d/src/object.cpp
View File

@ -1,65 +1,65 @@
#include <esim/imp_multi_objects_2d/object.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/common/time_conversions.hpp>
namespace event_camera_simulator {
bool loadTexture(const std::string& path_to_img, cv::Mat* img,
double median_blur, double gaussian_blur)
{
bool loadTexture(
const std::string& path_to_img,
cv::Mat* img,
double median_blur,
double gaussian_blur
) {
CHECK(img);
VLOG(1) << "Loading texture file from file: " << path_to_img << ".";
*img = cv::imread(path_to_img, cv::IMREAD_UNCHANGED);
if(!img->data)
{
if (!img->data) {
LOG(FATAL) << "Could not open image at: " << path_to_img << ".";
return false;
}
if(median_blur > 1)
{
if (median_blur > 1) {
VLOG(1) << "Pre-filtering the texture with median filter of size: "
<< median_blur << ".";
cv::medianBlur(*img, *img, median_blur);
}
if(gaussian_blur > 0)
{
VLOG(1) << "Pre-filtering the texture with gaussian filter of size: "
if (gaussian_blur > 0) {
VLOG(1
) << "Pre-filtering the texture with gaussian filter of size: "
<< gaussian_blur << ".";
cv::GaussianBlur(*img, *img, cv::Size(21,21), gaussian_blur);
cv::GaussianBlur(*img, *img, cv::Size(21, 21), gaussian_blur);
}
return true;
}
}
Object::Object(const std::string path_to_texture, const cv::Size& dst_size, const MotionParameters& motion_params,
double median_blur, double gaussian_blur)
Object::Object(
const std::string path_to_texture,
const cv::Size& dst_size,
const MotionParameters& motion_params,
double median_blur,
double gaussian_blur
)
: dst_size_(dst_size),
p_(motion_params)
{
loadTexture(path_to_texture, &texture_,
median_blur,
gaussian_blur);
p_(motion_params) {
loadTexture(path_to_texture, &texture_, median_blur, gaussian_blur);
K0_ << texture_.cols, 0, 0.5 * texture_.cols,
0, texture_.rows, 0.5 * texture_.rows,
0, 0, 1;
K0_ << texture_.cols, 0, 0.5 * texture_.cols, 0, texture_.rows,
0.5 * texture_.rows, 0, 0, 1;
K1_ << dst_size_.width, 0, 0.5 * dst_size_.width,
0, dst_size_.height, 0.5 * dst_size_.height,
0, 0, 1;
K1_ << dst_size_.width, 0, 0.5 * dst_size_.width, 0, dst_size_.height,
0.5 * dst_size_.height, 0, 0, 1;
canvas_ = cv::Mat(dst_size, CV_8UC4);
canvas_.setTo(0);
flow_ = OpticFlow(dst_size);
flow_.setTo(0);
}
}
void Object::draw(Time t, bool is_first_layer)
{
void Object::draw(Time t, bool is_first_layer) {
canvas_.setTo(0);
ze::real_t ts = ze::nanosecToSecTrunc(t);
@ -71,59 +71,61 @@ void Object::draw(Time t, bool is_first_layer)
Affine& dA10dt = A10_jac.second;
// test jacobian
// const ze::real_t h = 1e-5;
// AffineWithJacobian A = p_.getAffineTransformationWithJacobian(ts);
// AffineWithJacobian Ah = p_.getAffineTransformationWithJacobian(ts+h);
// Affine dAdt_numeric = 1./h * (Ah.first-A.first);
// const ze::real_t h = 1e-5;
// AffineWithJacobian A = p_.getAffineTransformationWithJacobian(ts);
// AffineWithJacobian Ah =
// p_.getAffineTransformationWithJacobian(ts+h); Affine dAdt_numeric
// = 1./h * (Ah.first-A.first);
// LOG(INFO) << dAdt_numeric;
// LOG(INFO) << A.second;
// LOG(INFO) << dAdt_numeric;
// LOG(INFO) << A.second;
Affine M_10 = K1_ * A10 * K0_.inv();
Affine dM10_dt = K1_ * dA10dt * K0_.inv();
// warpAffine requires M_dst_src unless the WARP_INVERSE_MAP flag is passed
// in which case it will require M_src_dst
// TODO: can we do something more efficient than fully warping the 4 channels (BGR+alpha)?
int border_mode = is_first_layer ? cv::BORDER_REFLECT101 : cv::BORDER_CONSTANT;
cv::warpPerspective(texture_,
// warpAffine requires M_dst_src unless the WARP_INVERSE_MAP flag is
// passed in which case it will require M_src_dst
// TODO: can we do something more efficient than fully warping the 4
// channels (BGR+alpha)?
int border_mode =
is_first_layer ? cv::BORDER_REFLECT101 : cv::BORDER_CONSTANT;
cv::warpPerspective(
texture_,
canvas_,
M_10,
canvas_.size(),
cv::INTER_LINEAR,
border_mode);
border_mode
);
cv::Matx<FloatType, 3, 3> SS = dM10_dt * M_10.inv();
for(int y=0; y<flow_.rows; ++y)
{
for(int x=0; x<flow_.cols; ++x)
{
flow_(y,x)[0] = SS(0,0) * x + SS(0,1) * y + SS(0,2);
flow_(y,x)[1] = SS(1,0) * x + SS(1,1) * y + SS(1,2);
for (int y = 0; y < flow_.rows; ++y) {
for (int x = 0; x < flow_.cols; ++x) {
flow_(y, x)[0] = SS(0, 0) * x + SS(0, 1) * y + SS(0, 2);
flow_(y, x)[1] = SS(1, 0) * x + SS(1, 1) * y + SS(1, 2);
}
}
}
}
void getIntensityAndAlpha(const cv::Mat& image,
int x, int y,
void getIntensityAndAlpha(
const cv::Mat& image,
int x,
int y,
ImageFloatType* intensity,
ImageFloatType* alpha)
{
ImageFloatType* alpha
) {
CHECK(image.type() == CV_8UC3 || image.type() == CV_8UC4);
if(image.type() == CV_8UC3)
{
cv::Vec3b val = image.at<cv::Vec3b>(y,x);
if (image.type() == CV_8UC3) {
cv::Vec3b val = image.at<cv::Vec3b>(y, x);
*intensity = bgrToGrayscale(val[0], val[1], val[2]) / 255.;
*alpha = 1.;
}
else
{
cv::Vec4b val = image.at<cv::Vec4b>(y,x);
} else {
cv::Vec4b val = image.at<cv::Vec4b>(y, x);
*intensity = bgrToGrayscale(val[0], val[1], val[2]) / 255.;
*alpha = static_cast<ImageFloatType>(val[3]) / 255.;
}
}
}
} // namespace event_camera_simulator

104
event_camera_simulator/imp/imp_opengl_renderer/include/esim/imp_opengl_renderer/opengl_renderer.hpp
View File

@ -8,10 +8,9 @@ class GLFWwindow; // fwd
namespace event_camera_simulator {
//! Rendering engine based on OpenGL
class OpenGLRenderer : public Renderer
{
public:
//! Rendering engine based on OpenGL
class OpenGLRenderer : public Renderer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ZE_POINTER_TYPEDEFS(Renderer);
@ -21,17 +20,23 @@ public:
~OpenGLRenderer();
//! Render an image at a given pose.
virtual void render(const Transformation& T_W_C,
virtual void render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap) const;
const DepthmapPtr& out_depthmap
) const;
//! Returns true if the rendering engine can compute optic flow, false otherwise
virtual bool canComputeOpticFlow() const override { return true; }
//! Returns true if the rendering engine can compute optic flow, false
//! otherwise
virtual bool canComputeOpticFlow() const override {
return true;
}
//! Render an image + depth map + optic flow map at a given pose,
//! given the camera linear and angular velocity
virtual void renderWithFlow(const Transformation& T_W_C,
virtual void renderWithFlow(
const Transformation& T_W_C,
const LinearVelocity& v_WC,
const AngularVelocity& w_WC,
const std::vector<Transformation>& T_W_OBJ,
@ -39,20 +44,23 @@ public:
const std::vector<AngularVelocity>& angular_velocity_obj,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap,
const OpticFlowPtr& optic_flow_map) const override;
const OpticFlowPtr& optic_flow_map
) const override;
//! Sets the camera
virtual void setCamera(const ze::Camera::Ptr& camera);
protected:
protected:
void init();
/**
@brief basic function to produce an OpenGL projection matrix and associated viewport parameters
which match a given set of camera intrinsics. This is currently written for the Eigen linear
algebra library, however it should be straightforward to port to any 4x4 matrix class.
@param[out] frustum Eigen::Matrix4d projection matrix. Eigen stores these matrices in column-major (i.e. OpenGL) order.
@brief basic function to produce an OpenGL projection matrix and
associated viewport parameters which match a given set of camera
intrinsics. This is currently written for the Eigen linear algebra
library, however it should be straightforward to port to any 4x4 matrix
class.
@param[out] frustum Eigen::Matrix4d projection matrix. Eigen stores
these matrices in column-major (i.e. OpenGL) order.
@param[in] alpha x-axis focal length, from camera intrinsic matrix
@param[in] alpha y-axis focal length, from camera intrinsic matrix
@param[in] skew x and y axis skew, from camera intrinsic matrix
@ -60,24 +68,37 @@ protected:
@param[in] v0 image origin y-coordinate, from camera intrinsic matrix
@param[in] img_width image width, in pixels
@param[in] img_height image height, in pixels
@param[in] near_clip near clipping plane z-location, can be set arbitrarily > 0, controls the mapping of z-coordinates for OpenGL
@param[in] far_clip far clipping plane z-location, can be set arbitrarily > near_clip, controls the mapping of z-coordinate for OpenGL
@param[in] near_clip near clipping plane z-location, can be set
arbitrarily > 0, controls the mapping of z-coordinates for OpenGL
@param[in] far_clip far clipping plane z-location, can be set
arbitrarily > near_clip, controls the mapping of z-coordinate for
OpenGL
Code adapted from:
- http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl/
- https://pastebin.com/h8nYNWJY
*/
void build_opengl_projection_for_intrinsics(Eigen::Matrix4d &frustum, double alpha, double beta, double u0, double v0, int img_width, int img_height, double near_clip, double far_clip) const {
// These parameters define the final viewport that is rendered into by
// the camera.
*/
void build_opengl_projection_for_intrinsics(
Eigen::Matrix4d& frustum,
double alpha,
double beta,
double u0,
double v0,
int img_width,
int img_height,
double near_clip,
double far_clip
) const {
// These parameters define the final viewport that is rendered into
// by the camera.
double L = 0;
double R = img_width;
double B = 0;
double T = img_height;
// near and far clipping planes, these only matter for the mapping from
// world-space z-coordinate into the depth coordinate for OpenGL
// near and far clipping planes, these only matter for the mapping
// from world-space z-coordinate into the depth coordinate for
// OpenGL
double N = near_clip;
double F = far_clip;
@ -88,25 +109,32 @@ protected:
// [-1, 1]. OpenGL then maps coordinates in NDC to the current
// viewport
Eigen::Matrix4d ortho = Eigen::Matrix4d::Zero();
ortho(0,0) = 2.0/(R-L); ortho(0,3) = -(R+L)/(R-L);
ortho(1,1) = 2.0/(T-B); ortho(1,3) = -(T+B)/(T-B);
ortho(2,2) = -2.0/(F-N); ortho(2,3) = -(F+N)/(F-N);
ortho(3,3) = 1.0;
ortho(0, 0) = 2.0 / (R - L);
ortho(0, 3) = -(R + L) / (R - L);
ortho(1, 1) = 2.0 / (T - B);
ortho(1, 3) = -(T + B) / (T - B);
ortho(2, 2) = -2.0 / (F - N);
ortho(2, 3) = -(F + N) / (F - N);
ortho(3, 3) = 1.0;
// construct a projection matrix, this is identical to the
// projection matrix computed for the intrinsicx, except an
// additional row is inserted to map the z-coordinate to
// OpenGL.
Eigen::Matrix4d tproj = Eigen::Matrix4d::Zero();
tproj(0,0) = alpha; tproj(0,1) = skew; tproj(0,2) = -u0;
tproj(1,1) = beta; tproj(1,2) = -v0;
tproj(2,2) = N+F; tproj(2,3) = N*F;
tproj(3,2) = -1.0;
tproj(0, 0) = alpha;
tproj(0, 1) = skew;
tproj(0, 2) = -u0;
tproj(1, 1) = beta;
tproj(1, 2) = -v0;
tproj(2, 2) = N + F;
tproj(2, 3) = N * F;
tproj(3, 2) = -1.0;
// resulting OpenGL frustum is the product of the orthographic
// mapping to normalized device coordinates and the augmented
// camera intrinsic matrix
frustum = ortho*tproj;
frustum = ortho * tproj;
}
GLFWwindow* window;
@ -124,12 +152,14 @@ protected:
unsigned int texture2;
unsigned int VBO, VAO;
unsigned int multisampled_fbo, multisampled_color_buf, multisampled_depth_buf;
unsigned int fbo, color_buf, depth_buf, depth_buf_of; // framebuffer for color and depth images
unsigned int multisampled_fbo, multisampled_color_buf,
multisampled_depth_buf;
unsigned int fbo, color_buf, depth_buf,
depth_buf_of; // framebuffer for color and depth images
unsigned int fbo_of, of_buf; // framebuffer for optic flow
float zmin = 0.1f;
float zmax = 10.0f;
};
};
} // namespace event_camera_simulator

392
event_camera_simulator/imp/imp_opengl_renderer/src/opengl_renderer.cpp
View File

@ -1,40 +1,46 @@
#include <esim/imp_opengl_renderer/opengl_renderer.hpp>
#include <glad/glad.h>
#include <learnopengl/shader.h>
#include <learnopengl/model.h>
#include <GLFW/glfw3.h>
#include <esim/imp_opengl_renderer/opengl_renderer.hpp>
#include <glad/glad.h>
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <learnopengl/filesystem.h>
#include <learnopengl/model.h>
#include <learnopengl/shader.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
namespace event_camera_simulator {
DEFINE_string(renderer_scene, "",
"Path to scene file (.obj) which will be used as simulation environment");
DEFINE_string(
renderer_scene,
"",
"Path to scene file (.obj) which will be used as simulation environment"
);
DEFINE_double(renderer_zmin, 0.1, "Near clipping distance.");
DEFINE_double(renderer_zmin, 0.1, "Near clipping distance.");
DEFINE_double(renderer_zmax, 10, "Far clipping distance.");
DEFINE_double(renderer_zmax, 10, "Far clipping distance.");
DEFINE_string(renderer_dynamic_objects_dir, "", "Path to directory that contains files of objects (.obj) that will be simulated");
DEFINE_string(renderer_dynamic_objects, "", "Files to be included as dynamic objects (.obj), separated by ;");
DEFINE_string(
renderer_dynamic_objects_dir,
"",
"Path to directory that contains files of objects (.obj) that "
"will be simulated"
);
DEFINE_string(
renderer_dynamic_objects,
"",
"Files to be included as dynamic objects (.obj), separated by ;"
);
OpenGLRenderer::OpenGLRenderer()
OpenGLRenderer::OpenGLRenderer()
: is_initialized_(false),
zmin(FLAGS_renderer_zmin),
zmax(FLAGS_renderer_zmax)
{
}
zmax(FLAGS_renderer_zmax) {}
void OpenGLRenderer::init()
{
void OpenGLRenderer::init() {
CHECK_GT(width_, 0);
CHECK_GT(height_, 0);
@ -47,14 +53,17 @@ void OpenGLRenderer::init()
glfwWindowHint(GLFW_VISIBLE, GL_FALSE);
#ifdef __APPLE__
glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // uncomment this statement to fix compilation on OS X
glfwWindowHint(
GLFW_OPENGL_FORWARD_COMPAT,
GL_TRUE
); // uncomment this statement to fix compilation on OS X
#endif
// glfw window creation
// --------------------
window = glfwCreateWindow(width_, height_, "OpenGLRenderer", NULL, NULL);
if (window == NULL)
{
window =
glfwCreateWindow(width_, height_, "OpenGLRenderer", NULL, NULL);
if (window == NULL) {
LOG(FATAL) << "Failed to create GLFW window";
glfwTerminate();
return;
@ -64,8 +73,7 @@ void OpenGLRenderer::init()
// glad: load all OpenGL function pointers
// ---------------------------------------
if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress))
{
if (!gladLoadGLLoader((GLADloadproc) glfwGetProcAddress)) {
LOG(FATAL) << "Failed to initialize GLAD";
return;
}
@ -74,25 +82,31 @@ void OpenGLRenderer::init()
// -----------
our_model.reset(new Model(FLAGS_renderer_scene));
// load dynamics objects
// ---------------------
if (!FLAGS_renderer_dynamic_objects_dir.empty() && !FLAGS_renderer_dynamic_objects.empty())
{
if (!FLAGS_renderer_dynamic_objects_dir.empty()
&& !FLAGS_renderer_dynamic_objects.empty()) {
size_t p_start, p_end;
p_start = 0;
while ((p_end = FLAGS_renderer_dynamic_objects.find(";",p_start)) != std::string::npos)
{
dynamic_objects_model.push_back(std::unique_ptr<Model>
(new Model(FLAGS_renderer_dynamic_objects_dir + "/" + FLAGS_renderer_dynamic_objects.substr(p_start, p_end - p_start))));
while ((p_end = FLAGS_renderer_dynamic_objects.find(";", p_start))
!= std::string::npos) {
dynamic_objects_model.push_back(
std::unique_ptr<Model>(new Model(
FLAGS_renderer_dynamic_objects_dir + "/"
+ FLAGS_renderer_dynamic_objects
.substr(p_start, p_end - p_start)
))
);
p_start = p_end + 1;
}
dynamic_objects_model.push_back(std::unique_ptr<Model>
(new Model(FLAGS_renderer_dynamic_objects_dir + "/" + FLAGS_renderer_dynamic_objects.substr(p_start, p_end - p_start))));
dynamic_objects_model.push_back(std::unique_ptr<Model>(new Model(
FLAGS_renderer_dynamic_objects_dir + "/"
+ FLAGS_renderer_dynamic_objects
.substr(p_start, p_end - p_start)
)));
}
// create multisampled framebuffer object
static const int num_samples = 4;
glGenFramebuffers(1, &multisampled_fbo);
@ -101,21 +115,42 @@ void OpenGLRenderer::init()
// create and attach a multisampled color buffer
glGenRenderbuffers(1, &multisampled_color_buf);
glBindRenderbuffer(GL_RENDERBUFFER, multisampled_color_buf);
glRenderbufferStorageMultisample(GL_RENDERBUFFER, num_samples, GL_RGBA8, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, multisampled_color_buf); // attach color buffer to FBO
glRenderbufferStorageMultisample(
GL_RENDERBUFFER,
num_samples,
GL_RGBA8,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_COLOR_ATTACHMENT0,
GL_RENDERBUFFER,
multisampled_color_buf
); // attach color buffer to FBO
// create and attach a multisampled depth buffer
glGenRenderbuffers(1, &multisampled_depth_buf);
glBindRenderbuffer(GL_RENDERBUFFER, multisampled_depth_buf);
glRenderbufferStorageMultisample(GL_RENDERBUFFER, num_samples, GL_DEPTH_COMPONENT24, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, multisampled_depth_buf); // attach depth buffer to FBO
glRenderbufferStorageMultisample(
GL_RENDERBUFFER,
num_samples,
GL_DEPTH_COMPONENT24,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER,
multisampled_depth_buf
); // attach depth buffer to FBO
GLenum status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
if(status != GL_FRAMEBUFFER_COMPLETE)
{
if (status != GL_FRAMEBUFFER_COMPLETE)
LOG(FATAL) << "ERROR: failed to set up multisampled framebuffer";
}
LOG(INFO) << "Successfully set up multisampled color and depth framebuffers";
LOG(INFO
) << "Successfully set up multisampled color and depth framebuffers";
// create framebuffer object
glGenFramebuffers(1, &fbo);
@ -124,20 +159,38 @@ void OpenGLRenderer::init()
// create and attach a color buffer
glGenRenderbuffers(1, &color_buf);
glBindRenderbuffer(GL_RENDERBUFFER, color_buf);
glRenderbufferStorage(GL_RENDERBUFFER, GL_RGBA8, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, color_buf); // attach color buffer to FBO
glRenderbufferStorage(
GL_RENDERBUFFER,
GL_RGBA8,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_COLOR_ATTACHMENT0,
GL_RENDERBUFFER,
color_buf
); // attach color buffer to FBO
// create and attach a depth buffer
glGenRenderbuffers(1, &depth_buf);
glBindRenderbuffer(GL_RENDERBUFFER, depth_buf);
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, depth_buf); // attach depth buffer to FBO
glRenderbufferStorage(
GL_RENDERBUFFER,
GL_DEPTH_COMPONENT24,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER,
depth_buf
); // attach depth buffer to FBO
status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
if(status != GL_FRAMEBUFFER_COMPLETE)
{
if (status != GL_FRAMEBUFFER_COMPLETE)
LOG(FATAL) << "ERROR: failed to set up framebuffer";
}
LOG(INFO) << "Successfully set up color and depth framebuffers";
// create framebuffer object for optic flow
@ -147,37 +200,58 @@ void OpenGLRenderer::init()
// create and attach a color buffer for optic flow
glGenRenderbuffers(1, &of_buf);
glBindRenderbuffer(GL_RENDERBUFFER, of_buf);
glRenderbufferStorage(GL_RENDERBUFFER, GL_RGBA32F, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, of_buf); // attach optic flow buffer to frame buffer object
glRenderbufferStorage(
GL_RENDERBUFFER,
GL_RGBA32F,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_COLOR_ATTACHMENT0,
GL_RENDERBUFFER,
of_buf
); // attach optic flow buffer to frame buffer object
// create and attach a depth buffer for optic flow
glGenRenderbuffers(1, &depth_buf_of);
glBindRenderbuffer(GL_RENDERBUFFER, depth_buf_of);
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, width_, height_); // set format
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, depth_buf_of); // attach depth buffer to FBO
glRenderbufferStorage(
GL_RENDERBUFFER,
GL_DEPTH_COMPONENT24,
width_,
height_
); // set format
glFramebufferRenderbuffer(
GL_FRAMEBUFFER,
GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER,
depth_buf_of
); // attach depth buffer to FBO
GLenum status_of = glCheckFramebufferStatus(GL_FRAMEBUFFER);
if(status_of != GL_FRAMEBUFFER_COMPLETE)
{
if (status_of != GL_FRAMEBUFFER_COMPLETE)
LOG(FATAL) << "ERROR: failed to set up framebuffer for optic flow";
}
LOG(INFO) << "Successfully set up optic flow framebuffer";
// create shader program
shader.reset(new Shader(FileSystem::getPath("src/shader.vert").c_str(),
FileSystem::getPath("src/shader.frag").c_str()));
shader.reset(new Shader(
FileSystem::getPath("src/shader.vert").c_str(),
FileSystem::getPath("src/shader.frag").c_str()
));
optic_flow_shader.reset(new Shader(FileSystem::getPath("src/shader.vert").c_str(),
FileSystem::getPath("src/optic_flow_shader.frag").c_str()));
optic_flow_shader.reset(new Shader(
FileSystem::getPath("src/shader.vert").c_str(),
FileSystem::getPath("src/optic_flow_shader.frag").c_str()
));
glEnable(GL_DEPTH_TEST);
glEnable(GL_MULTISAMPLE);
is_initialized_ = true;
}
}
OpenGLRenderer::~OpenGLRenderer()
{
OpenGLRenderer::~OpenGLRenderer() {
// cleanup
glDeleteFramebuffers(1, &multisampled_fbo);
glDeleteFramebuffers(1, &fbo);
@ -193,32 +267,37 @@ OpenGLRenderer::~OpenGLRenderer()
// glfw: terminate, clearing all previously allocated GLFW resources.
// ------------------------------------------------------------------
glfwTerminate();
}
}
void OpenGLRenderer::setCamera(const ze::Camera::Ptr& camera)
{
CHECK(!is_initialized_) << "setCamera() was called but the OpenGL renderer was already setup before";
void OpenGLRenderer::setCamera(const ze::Camera::Ptr& camera) {
CHECK(!is_initialized_)
<< "setCamera() was called but the OpenGL renderer "
"was already setup before";
CHECK(camera);
camera_ = camera;
width_ = camera_->width();
height_ = camera_->height();
if(camera->type() != ze::CameraType::Pinhole)
{
LOG(WARNING) << "The OpenGL rendering engine does not support camera distortion. The distortion parameters will be ignored.";
if (camera->type() != ze::CameraType::Pinhole) {
LOG(WARNING
) << "The OpenGL rendering engine does not support camera "
"distortion. The distortion parameters will be ignored.";
// TODO: actually remove distortion from the camera so
// that they do not get published on the camera_info topic
}
init();
}
}
void OpenGLRenderer::render(const Transformation& T_W_C,
void OpenGLRenderer::render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap) const
{
CHECK(is_initialized_) << "Called render() but the renderer was not initialized yet. Have you first called setCamera()?";
const DepthmapPtr& out_depthmap
) const {
CHECK(is_initialized_) << "Called render() but the renderer was not "
"initialized yet. Have you "
"first called setCamera()?";
CHECK(out_image);
CHECK(out_depthmap);
CHECK_EQ(out_image->cols, width_);
@ -242,11 +321,10 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
// We invert the Z axis here because NDC coordinates
// are left-handed by default in OpenGL
// (see https://stackoverflow.com/a/12336360)
T_C_W.block<1,4>(2,0) *= -1.0;
T_C_W.block<1, 4>(2, 0) *= -1.0;
// view = transformation from point in world to point in camera
glm::mat4 view =
glm::make_mat4(T_C_W.data());
glm::mat4 view = glm::make_mat4(T_C_W.data());
ze::Matrix4 frustum;
ze::VectorX intrinsics = camera_->projectionParameters();
@ -254,7 +332,17 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
const double fy = intrinsics(1);
const double cx = intrinsics(2);
const double cy = intrinsics(3);
build_opengl_projection_for_intrinsics(frustum, fx, fy, cx, cy, width_, height_, zmin, zmax);
build_opengl_projection_for_intrinsics(
frustum,
fx,
fy,
cx,
cy,
width_,
height_,
zmin,
zmax
);
glm::mat4 projection = glm::make_mat4(frustum.data());
unsigned int modelLoc = glGetUniformLocation(shader->ID, "model");
@ -263,14 +351,19 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
unsigned int viewLoc = glGetUniformLocation(shader->ID, "view");
glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view));
unsigned int projectionLoc = glGetUniformLocation(shader->ID, "projection");
glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(projection)); // TODO outside of main loop
unsigned int projectionLoc =
glGetUniformLocation(shader->ID, "projection");
glUniformMatrix4fv(
projectionLoc,
1,
GL_FALSE,
glm::value_ptr(projection)
); // TODO outside of main loop
our_model->Draw(*shader);
// draw dynamic objects
for (size_t i = 0; i < dynamic_objects_model.size(); i++)
{
for (size_t i = 0; i < dynamic_objects_model.size(); i++) {
ze::Matrix4 T_W_OBJ_OPENGL = T_W_OBJ[i].getTransformationMatrix();
model = glm::make_mat4(T_W_OBJ_OPENGL.data());
@ -281,17 +374,39 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
// now resolve multisampled buffer into the normal fbo
glBindFramebuffer(GL_READ_FRAMEBUFFER, multisampled_fbo);
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, fbo);
glBlitFramebuffer(0, 0, width_, height_, 0, 0, width_, height_, GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT, GL_NEAREST);
glBlitFramebuffer(
0,
0,
width_,
height_,
0,
0,
width_,
height_,
GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT,
GL_NEAREST
);
// bind fbo back so that we read from it
glBindFramebuffer(GL_FRAMEBUFFER, fbo);
// read out what we just rendered
cv::Mat img_color(height_, width_, CV_8UC3);
glPixelStorei(GL_PACK_ALIGNMENT, (img_color.step & 3)?1:4);
glPixelStorei(GL_PACK_ROW_LENGTH, img_color.step/img_color.elemSize());
glPixelStorei(GL_PACK_ALIGNMENT, (img_color.step & 3) ? 1 : 4);
glPixelStorei(
GL_PACK_ROW_LENGTH,
img_color.step / img_color.elemSize()
);
glReadPixels(0, 0, img_color.cols, img_color.rows, GL_BGR, GL_UNSIGNED_BYTE, img_color.data);
glReadPixels(
0,
0,
img_color.cols,
img_color.rows,
GL_BGR,
GL_UNSIGNED_BYTE,
img_color.data
);
GLenum err = glGetError();
if (err) {
@ -301,10 +416,21 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
// read out depth data
cv::Mat img_depth(height_, width_, CV_32FC1);
glPixelStorei(GL_PACK_ALIGNMENT, (img_depth.step & 3)?1:4);
glPixelStorei(GL_PACK_ROW_LENGTH, img_depth.step/img_depth.elemSize());
glPixelStorei(GL_PACK_ALIGNMENT, (img_depth.step & 3) ? 1 : 4);
glPixelStorei(
GL_PACK_ROW_LENGTH,
img_depth.step / img_depth.elemSize()
);
glReadPixels(0, 0, img_depth.cols, img_depth.rows, GL_DEPTH_COMPONENT, GL_FLOAT, img_depth.data);
glReadPixels(
0,
0,
img_depth.cols,
img_depth.rows,
GL_DEPTH_COMPONENT,
GL_FLOAT,
img_depth.data
);
err = glGetError();
if (err) {
@ -314,17 +440,19 @@ void OpenGLRenderer::render(const Transformation& T_W_C,
// convert inverse depth buffer to linear depth between zmin and zmax
// see the "Learn OpenGL book, page 177
cv::Mat linear_depth = (2.0 * zmin * zmax) / (zmax + zmin - (2 * img_depth - 1.f) * (zmax - zmin));
cv::Mat linear_depth =
(2.0 * zmin * zmax)
/ (zmax + zmin - (2 * img_depth - 1.f) * (zmax - zmin));
cv::Mat img_grayscale;
cv::cvtColor(img_color, img_grayscale, cv::COLOR_BGR2GRAY);
img_grayscale.convertTo(*out_image, CV_32F, 1.f/255.f);
img_grayscale.convertTo(*out_image, CV_32F, 1.f / 255.f);
linear_depth.copyTo(*out_depthmap);
}
}
void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
void OpenGLRenderer::renderWithFlow(
const Transformation& T_W_C,
const LinearVelocity& v_WC,
const AngularVelocity& w_WC,
const std::vector<Transformation>& T_W_OBJ,
@ -332,8 +460,8 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
const std::vector<AngularVelocity>& angular_velocity_obj,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap,
const OpticFlowPtr& optic_flow_map) const
{
const OpticFlowPtr& optic_flow_map
) const {
render(T_W_C, T_W_OBJ, out_image, out_depthmap);
// draw to our optic flow framebuffer instead of screen
@ -352,11 +480,10 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
// We invert the Z axis here because NDC coordinates
// are left-handed by default in OpenGL
// (see https://stackoverflow.com/a/12336360)
T_C_W_tilde.block<1,4>(2,0) *= -1.0;
T_C_W_tilde.block<1, 4>(2, 0) *= -1.0;
// view = transformation from point in world to point in camera
glm::mat4 view =
glm::make_mat4(T_C_W_tilde.data());
glm::mat4 view = glm::make_mat4(T_C_W_tilde.data());
ze::Matrix4 frustum;
ze::VectorX intrinsics = camera_->projectionParameters();
@ -364,17 +491,35 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
const double fy = intrinsics(1);
const double cx = intrinsics(2);
const double cy = intrinsics(3);
build_opengl_projection_for_intrinsics(frustum, fx, fy, cx, cy, width_, height_, zmin, zmax);
build_opengl_projection_for_intrinsics(
frustum,
fx,
fy,
cx,
cy,
width_,
height_,
zmin,
zmax
);
glm::mat4 projection = glm::make_mat4(frustum.data());
unsigned int modelLoc = glGetUniformLocation(optic_flow_shader->ID, "model");
unsigned int modelLoc =
glGetUniformLocation(optic_flow_shader->ID, "model");
glUniformMatrix4fv(modelLoc, 1, GL_FALSE, glm::value_ptr(model));
unsigned int viewLoc = glGetUniformLocation(optic_flow_shader->ID, "view");
unsigned int viewLoc =
glGetUniformLocation(optic_flow_shader->ID, "view");
glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(view));
unsigned int projectionLoc = glGetUniformLocation(optic_flow_shader->ID, "projection");
glUniformMatrix4fv(projectionLoc, 1, GL_FALSE, glm::value_ptr(projection)); // TODO, remove from main loop
unsigned int projectionLoc =
glGetUniformLocation(optic_flow_shader->ID, "projection");
glUniformMatrix4fv(
projectionLoc,
1,
GL_FALSE,
glm::value_ptr(projection)
); // TODO, remove from main loop
// compute optic flow and draw it to the screen buffer
glm::vec3 w = glm::make_vec3(w_WC.data());
@ -398,23 +543,23 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
our_model->Draw(*optic_flow_shader);
// draw optical flow for dynamic objects
for (size_t i = 0; i < dynamic_objects_model.size(); i++)
{
for (size_t i = 0; i < dynamic_objects_model.size(); i++) {
optic_flow_shader->setBool("dynamic_object", true);
// relative position (in camera frame)
ze::Vector3 C_r_C_OBJ = (T_C_W*T_W_OBJ[i]).getPosition();
ze::Vector3 C_r_C_OBJ = (T_C_W * T_W_OBJ[i]).getPosition();
glm::vec3 r_C_OBJ = glm::make_vec3(C_r_C_OBJ.data());
optic_flow_shader->setVec3("r_CB", r_C_OBJ);
// linear velocity (in camera frame)
ze::Vector3 C_v_OBJ = T_C_W.getRotation().rotate(linear_velocity_obj[i]);
ze::Vector3 C_v_OBJ =
T_C_W.getRotation().rotate(linear_velocity_obj[i]);
glm::vec3 v_obj = glm::make_vec3(C_v_OBJ.data());
optic_flow_shader->setVec3("v_WB", v_obj);
// angular velocity (in camera frame)
ze::Vector3 C_w_OBJ = T_C_W.getRotation().rotate(angular_velocity_obj[i]);
ze::Vector3 C_w_OBJ =
T_C_W.getRotation().rotate(angular_velocity_obj[i]);
glm::vec3 w_obj = glm::make_vec3(C_w_OBJ.data());
optic_flow_shader->setVec3("w_WB", w_obj);
@ -422,14 +567,13 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
model = glm::make_mat4(T_W_OBJ_OPENGL.data());
optic_flow_shader->setMat4("model", model);
dynamic_objects_model[i]->Draw(*optic_flow_shader);
}
// read out the optic flow we just rendered
cv::Mat flow(height_, width_, CV_32FC3);
glPixelStorei(GL_PACK_ALIGNMENT, (flow.step & 3)?1:4);
glPixelStorei(GL_PACK_ROW_LENGTH, flow.step/flow.elemSize());
glPixelStorei(GL_PACK_ALIGNMENT, (flow.step & 3) ? 1 : 4);
glPixelStorei(GL_PACK_ROW_LENGTH, flow.step / flow.elemSize());
glReadPixels(0, 0, flow.cols, flow.rows, GL_BGR, GL_FLOAT, flow.data);
@ -439,16 +583,14 @@ void OpenGLRenderer::renderWithFlow(const Transformation& T_W_C,
return;
}
for(int y=0; y<height_; ++y)
{
for(int x=0; x<width_; ++x)
{
(*optic_flow_map)(y,x)
= cv::Vec<ImageFloatType,2>(flow.at<cv::Vec<ImageFloatType,3>>(y,x)[2],
flow.at<cv::Vec<ImageFloatType,3>>(y,x)[1]);
for (int y = 0; y < height_; ++y) {
for (int x = 0; x < width_; ++x) {
(*optic_flow_map)(y, x) = cv::Vec<ImageFloatType, 2>(
flow.at<cv::Vec<ImageFloatType, 3>>(y, x)[2],
flow.at<cv::Vec<ImageFloatType, 3>>(y, x)[1]
);
}
}
}
}
} // namespace event_camera_simulator

42
event_camera_simulator/imp/imp_panorama_renderer/include/esim/imp_panorama_renderer/panorama_renderer.hpp
View File

@ -4,34 +4,46 @@
namespace event_camera_simulator {
//! A rendering engine for planar scenes
class PanoramaRenderer : public Renderer
{
public:
//! A rendering engine for planar scenes
class PanoramaRenderer : public Renderer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PanoramaRenderer(const Image& texture,
const Transformation::Rotation& R_W_P);
PanoramaRenderer(
const Image& texture, const Transformation::Rotation& R_W_P
);
~PanoramaRenderer();
//! Render image and depth map for a given camera pose
virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const;
virtual void render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const;
void render(const Transformation& T_W_C, const std::vector<Transformation>& T_W_OBJ, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const override
{
void render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const override {
render(T_W_C, out_image, out_depthmap);
}
//! Returns true if the rendering engine can compute optic flow, false otherwise
virtual bool canComputeOpticFlow() const override { return false; }
//! Returns true if the rendering engine can compute optic flow, false
//! otherwise
virtual bool canComputeOpticFlow() const override {
return false;
}
virtual void setCamera(const ze::Camera::Ptr& camera) override;
protected:
protected:
void precomputePixelToBearingLookupTable();
void projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const;
void projectToPanorama(
const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint
) const;
// Texture mapped on the plane
Image texture_;
@ -46,6 +58,6 @@ protected:
// Preallocated warping matrices
mutable cv::Mat_<float> map_x_, map_y_;
};
};
} // namespace event_camera_simulator

98
event_camera_simulator/imp/imp_panorama_renderer/src/panorama_renderer.cpp
View File

@ -1,34 +1,30 @@
#include <esim/common/utils.hpp>
#include <esim/imp_panorama_renderer/panorama_renderer.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
namespace event_camera_simulator {
PanoramaRenderer::PanoramaRenderer(const Image& texture,
const Transformation::Rotation &R_W_P)
PanoramaRenderer::PanoramaRenderer(
const Image& texture, const Transformation::Rotation& R_W_P
)
: texture_(texture),
R_W_P_(R_W_P)
{
principal_point_ = ze::Keypoint(0.5 * texture_.cols,
0.5 * texture_.rows);
R_W_P_(R_W_P) {
principal_point_ =
ze::Keypoint(0.5 * texture_.cols, 0.5 * texture_.rows);
fx_ = texture_.cols / (2.0 * CV_PI);
fy_ = texture_.rows / CV_PI;
LOG(INFO) << "Initialized panoramic camera with size: "
<< texture_.cols << "x" << texture_.rows
LOG(INFO) << "Initialized panoramic camera with size: " << texture_.cols
<< "x" << texture_.rows
<< ", principal point: " << principal_point_.transpose()
<< ", fx = " << fx_ << "px , fy = " << fy_ << " px";
}
}
PanoramaRenderer::~PanoramaRenderer()
{
}
PanoramaRenderer::~PanoramaRenderer() {}
void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera)
{
void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera) {
CHECK(camera);
camera_ = camera;
@ -38,27 +34,22 @@ void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera)
cv::Size sensor_size(camera->width(), camera->height());
map_x_ = cv::Mat_<float>::zeros(sensor_size);
map_y_ = cv::Mat_<float>::zeros(sensor_size);
}
}
void PanoramaRenderer::precomputePixelToBearingLookupTable()
{
// points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
void PanoramaRenderer::precomputePixelToBearingLookupTable() {
// points_C is a matrix containing the coordinates of each pixel
// coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
}
}
for (int y = 0; y < camera_->height(); ++y)
for (int x = 0; x < camera_->width(); ++x)
points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y);
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
}
void PanoramaRenderer::projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const
{
void PanoramaRenderer::projectToPanorama(
const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint
) const {
CHECK(keypoint);
static constexpr FloatType kEpsilon = 1e-10;
@ -66,48 +57,55 @@ void PanoramaRenderer::projectToPanorama(const Eigen::Ref<const ze::Bearing>& f,
const FloatType Y = f[1];
const FloatType Z = f[2];
const FloatType rho2 = X*X+Y*Y+Z*Z;
const FloatType rho2 = X * X + Y * Y + Z * Z;
const FloatType rho = std::sqrt(rho2);
CHECK_GE(rho, kEpsilon);
const FloatType phi = std::atan2(X, Z);
const FloatType theta = std::asin(-Y/rho);
const FloatType theta = std::asin(-Y / rho);
*keypoint = principal_point_ + ze::Keypoint(phi * fx_, -theta * fy_);
}
}
void PanoramaRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
{
void PanoramaRenderer::render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const {
CHECK_EQ(out_image->rows, camera_->height());
CHECK_EQ(out_image->cols, camera_->width());
const Transformation::RotationMatrix R_P_C = R_W_P_.inverse().getRotationMatrix() * T_W_C.getRotationMatrix();
const Transformation::RotationMatrix R_P_C =
R_W_P_.inverse().getRotationMatrix() * T_W_C.getRotationMatrix();
ze::Bearings bearings_P = R_P_C * bearings_C_;
ze::Keypoint keypoint;
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const ze::Bearing& f = bearings_P.col(x + camera_->width() * y);
// project bearing vector to panorama image
projectToPanorama(f, &keypoint);
map_x_(y,x) = static_cast<float>(keypoint[0]);
map_y_(y,x) = static_cast<float>(keypoint[1]);
map_x_(y, x) = static_cast<float>(keypoint[0]);
map_y_(y, x) = static_cast<float>(keypoint[1]);
}
}
cv::remap(texture_, *out_image, map_x_, map_y_, cv::INTER_LINEAR, cv::BORDER_REFLECT_101);
cv::remap(
texture_,
*out_image,
map_x_,
map_y_,
cv::INTER_LINEAR,
cv::BORDER_REFLECT_101
);
if(out_depthmap)
{
if (out_depthmap) {
static constexpr ImageFloatType infinity = 1e10;
out_depthmap->setTo(infinity);
}
}
}
} // namespace event_camera_simulator

27
event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp
View File

@ -4,26 +4,31 @@
namespace event_camera_simulator {
//! A rendering engine for planar scenes
class PanoramaRenderer : public Renderer
{
public:
//! A rendering engine for planar scenes
class PanoramaRenderer : public Renderer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PanoramaRenderer(const Image& texture,
const Transformation::Rotation& R_W_P);
PanoramaRenderer(
const Image& texture, const Transformation::Rotation& R_W_P
);
~PanoramaRenderer();
//! Render image and depth map for a given camera pose
virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const override;
virtual void render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const override;
virtual void setCamera(const ze::Camera::Ptr& camera) override;
protected:
protected:
void precomputePixelToBearingLookupTable();
void projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const;
void projectToPanorama(
const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint
) const;
// Texture mapped on the plane
Image texture_;
@ -38,6 +43,6 @@ protected:
// Preallocated warping matrices
mutable cv::Mat_<float> map_x_, map_y_;
};
};
} // namespace event_camera_simulator

42
event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp
View File

@ -4,35 +4,47 @@
namespace event_camera_simulator {
//! A rendering engine for planar scenes
class PlanarRenderer : public Renderer
{
public:
//! A rendering engine for planar scenes
class PlanarRenderer : public Renderer {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PlanarRenderer(const Image& texture,
PlanarRenderer(
const Image& texture,
const Camera::Ptr& cam_src,
const Transformation &T_W_P,
const Transformation& T_W_P,
FloatType z_min,
FloatType z_max,
bool extend_border);
bool extend_border
);
~PlanarRenderer();
//! Render image and depth map for a given camera pose
virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const;
void render(const Transformation& T_W_C, const std::vector<Transformation>& T_W_OBJ, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const override
{
virtual void render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const;
void render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const override {
render(T_W_C, out_image, out_depthmap);
}
//! Returns true if the rendering engine can compute optic flow, false otherwise
virtual bool canComputeOpticFlow() const override { return false; }
//! Returns true if the rendering engine can compute optic flow, false
//! otherwise
virtual bool canComputeOpticFlow() const override {
return false;
}
virtual void setCamera(const ze::Camera::Ptr& camera) override;
protected:
protected:
void precomputePixelToBearingLookupTable();
// Texture mapped on the plane
@ -54,6 +66,6 @@ protected:
// Preallocated warping matrices
mutable cv::Mat_<float> map_x_, map_y_;
};
};
} // namespace event_camera_simulator

33
event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp
View File

@ -1,42 +1,47 @@
#pragma once
#include <ze/common/macros.hpp>
#include <esim/common/types.hpp>
#include <ze/common/macros.hpp>
namespace event_camera_simulator {
//! Represents a rendering engine that generates images (and other outputs,
//! such as depth maps, or optical flow maps) given a scene and a camera position.
class Renderer
{
public:
//! Represents a rendering engine that generates images (and other outputs,
//! such as depth maps, or optical flow maps) given a scene and a camera
//! position.
class Renderer {
public:
ZE_POINTER_TYPEDEFS(Renderer);
Renderer() {}
//! Render an image at a given pose.
virtual void render(const Transformation& T_W_C,
virtual void render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap) const = 0;
const DepthmapPtr& out_depthmap
) const = 0;
//! Render an image + depth map + optic flow map at a given pose,
//! given the camera linear and angular velocity
virtual void render(const Transformation& T_W_C,
virtual void render(
const Transformation& T_W_C,
const LinearVelocity& v_WC,
const AngularVelocity& w_WC,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap,
const OpticFlowPtr& optic_flow_map) const {}
const OpticFlowPtr& optic_flow_map
) const {}
//! Sets the camera
virtual void setCamera(const ze::Camera::Ptr& camera) = 0;
//! Get the camera rig
const ze::Camera::Ptr& getCamera() const { return camera_; }
const ze::Camera::Ptr& getCamera() const {
return camera_;
}
protected:
protected:
ze::Camera::Ptr camera_;
};
};
} // namespace event_camera_simulator

6
event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp
View File

@ -1,11 +1,11 @@
#pragma once
#include <gflags/gflags.h>
#include <esim/rendering/renderer_base.hpp>
#include <gflags/gflags.h>
namespace event_camera_simulator {
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat *img);
Renderer::Ptr loadRendererFromGflags();
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img);
Renderer::Ptr loadRendererFromGflags();
} // namespace event_camera_simulator

98
event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp
View File

@ -1,34 +1,30 @@
#include <esim/common/utils.hpp>
#include <esim/rendering/panorama_renderer.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
namespace event_camera_simulator {
PanoramaRenderer::PanoramaRenderer(const Image& texture,
const Transformation::Rotation &R_W_P)
PanoramaRenderer::PanoramaRenderer(
const Image& texture, const Transformation::Rotation& R_W_P
)
: texture_(texture),
R_W_P_(R_W_P)
{
principal_point_ = ze::Keypoint(0.5 * texture_.cols,
0.5 * texture_.rows);
R_W_P_(R_W_P) {
principal_point_ =
ze::Keypoint(0.5 * texture_.cols, 0.5 * texture_.rows);
fx_ = texture_.cols / (2.0 * CV_PI);
fy_ = texture_.rows / CV_PI;
LOG(INFO) << "Initialized panoramic camera with size: "
<< texture_.cols << "x" << texture_.rows
LOG(INFO) << "Initialized panoramic camera with size: " << texture_.cols
<< "x" << texture_.rows
<< ", principal point: " << principal_point_.transpose()
<< ", fx = " << fx_ << "px , fy = " << fy_ << " px";
}
}
PanoramaRenderer::~PanoramaRenderer()
{
}
PanoramaRenderer::~PanoramaRenderer() {}
void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera)
{
void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera) {
CHECK(camera);
camera_ = camera;
@ -38,27 +34,22 @@ void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera)
cv::Size sensor_size(camera->width(), camera->height());
map_x_ = cv::Mat_<float>::zeros(sensor_size);
map_y_ = cv::Mat_<float>::zeros(sensor_size);
}
}
void PanoramaRenderer::precomputePixelToBearingLookupTable()
{
// points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
void PanoramaRenderer::precomputePixelToBearingLookupTable() {
// points_C is a matrix containing the coordinates of each pixel
// coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
}
}
for (int y = 0; y < camera_->height(); ++y)
for (int x = 0; x < camera_->width(); ++x)
points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y);
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
}
void PanoramaRenderer::projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const
{
void PanoramaRenderer::projectToPanorama(
const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint
) const {
CHECK(keypoint);
static constexpr FloatType kEpsilon = 1e-10;
@ -66,48 +57,55 @@ void PanoramaRenderer::projectToPanorama(const Eigen::Ref<const ze::Bearing>& f,
const FloatType Y = f[1];
const FloatType Z = f[2];
const FloatType rho2 = X*X+Y*Y+Z*Z;
const FloatType rho2 = X * X + Y * Y + Z * Z;
const FloatType rho = std::sqrt(rho2);
CHECK_GE(rho, kEpsilon);
const FloatType phi = std::atan2(X, Z);
const FloatType theta = std::asin(-Y/rho);
const FloatType theta = std::asin(-Y / rho);
*keypoint = principal_point_ + ze::Keypoint(phi * fx_, -theta * fy_);
}
}
void PanoramaRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
{
void PanoramaRenderer::render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const {
CHECK_EQ(out_image->rows, camera_->height());
CHECK_EQ(out_image->cols, camera_->width());
const Transformation::RotationMatrix R_P_C = R_W_P_.inverse().getRotationMatrix() * T_W_C.getRotationMatrix();
const Transformation::RotationMatrix R_P_C =
R_W_P_.inverse().getRotationMatrix() * T_W_C.getRotationMatrix();
ze::Bearings bearings_P = R_P_C * bearings_C_;
ze::Keypoint keypoint;
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const ze::Bearing& f = bearings_P.col(x + camera_->width() * y);
// project bearing vector to panorama image
projectToPanorama(f, &keypoint);
map_x_(y,x) = static_cast<float>(keypoint[0]);
map_y_(y,x) = static_cast<float>(keypoint[1]);
map_x_(y, x) = static_cast<float>(keypoint[0]);
map_y_(y, x) = static_cast<float>(keypoint[1]);
}
}
cv::remap(texture_, *out_image, map_x_, map_y_, CV_INTER_LINEAR, cv::BORDER_REFLECT_101);
cv::remap(
texture_,
*out_image,
map_x_,
map_y_,
CV_INTER_LINEAR,
cv::BORDER_REFLECT_101
);
if(out_depthmap)
{
if (out_depthmap) {
static constexpr ImageFloatType infinity = 1e10;
out_depthmap->setTo(infinity);
}
}
}
} // namespace event_camera_simulator

126
event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp
View File

@ -1,48 +1,48 @@
#include <esim/common/utils.hpp>
#include <esim/imp_planar_renderer/planar_renderer.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <glog/logging.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/common/timer_collection.hpp>
DECLARE_TIMER(TimerPlanarRenderer, timers_planar_renderer,
DECLARE_TIMER(
TimerPlanarRenderer,
timers_planar_renderer,
compute_remap_maps,
inverse_homography,
remap,
compute_depthmap
);
);
namespace event_camera_simulator {
PlanarRenderer::PlanarRenderer(const Image& texture,
const Camera::Ptr &cam_src,
PlanarRenderer::PlanarRenderer(
const Image& texture,
const Camera::Ptr& cam_src,
const Transformation& T_W_P,
FloatType z_min,
FloatType z_max,
bool extend_border)
bool extend_border
)
: texture_(texture),
cam_src_(cam_src),
T_W_P_(T_W_P),
z_min_(z_min),
z_max_(z_max),
extend_border_(extend_border)
{
extend_border_(extend_border) {
K_src_ = calibrationMatrixFromCamera(cam_src_);
K_src_inv_ = K_src_.inverse();
VLOG(1) << "K_src = " << K_src_;
LOG(INFO) << "T_W_P = " << T_W_P_;
}
}
PlanarRenderer::~PlanarRenderer()
{
PlanarRenderer::~PlanarRenderer() {
timers_planar_renderer.saveToFile("/tmp", "planar_renderer.csv");
}
}
void PlanarRenderer::setCamera(const ze::Camera::Ptr& camera)
{
void PlanarRenderer::setCamera(const ze::Camera::Ptr& camera) {
CHECK(camera);
camera_ = camera;
@ -52,26 +52,24 @@ void PlanarRenderer::setCamera(const ze::Camera::Ptr& camera)
cv::Size sensor_size(camera->width(), camera->height());
map_x_ = cv::Mat_<float>::zeros(sensor_size);
map_y_ = cv::Mat_<float>::zeros(sensor_size);
}
}
void PlanarRenderer::precomputePixelToBearingLookupTable()
{
// points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
void PlanarRenderer::precomputePixelToBearingLookupTable() {
// points_C is a matrix containing the coordinates of each pixel
// coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
}
}
for (int y = 0; y < camera_->height(); ++y)
for (int x = 0; x < camera_->width(); ++x)
points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y);
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
}
void PlanarRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
{
void PlanarRenderer::render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const {
CHECK_EQ(out_image->rows, camera_->height());
CHECK_EQ(out_image->cols, camera_->width());
@ -90,57 +88,71 @@ void PlanarRenderer::render(const Transformation& T_W_C, const ImagePtr& out_ima
HomographyMatrix H_P_C;
{
auto t = timers_planar_renderer[TimerPlanarRenderer::inverse_homography].timeScope();
auto t =
timers_planar_renderer[TimerPlanarRenderer::inverse_homography]
.timeScope();
H_P_C = H_C_P.inverse();
}
{
auto t = timers_planar_renderer[TimerPlanarRenderer::compute_remap_maps].timeScope();
auto t =
timers_planar_renderer[TimerPlanarRenderer::compute_remap_maps]
.timeScope();
ze::Bearings bearings_P = H_P_C * bearings_C_;
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
const ze::Bearing& f = bearings_P.col(x + camera_->width() * y);
map_x_(y,x) = static_cast<float>(K_src_(0,0) * f[0]/f[2] + K_src_(0,2));
map_y_(y,x) = static_cast<float>(K_src_(1,1) * f[1]/f[2] + K_src_(1,2));
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const ze::Bearing& f =
bearings_P.col(x + camera_->width() * y);
map_x_(y, x) = static_cast<float>(
K_src_(0, 0) * f[0] / f[2] + K_src_(0, 2)
);
map_y_(y, x) = static_cast<float>(
K_src_(1, 1) * f[1] / f[2] + K_src_(1, 2)
);
}
}
}
int border = (extend_border_) ? cv::BORDER_REFLECT : cv::BORDER_CONSTANT;
int border =
(extend_border_) ? cv::BORDER_REFLECT : cv::BORDER_CONSTANT;
{
auto t = timers_planar_renderer[TimerPlanarRenderer::remap].timeScope();
cv::remap(texture_, *out_image, map_x_, map_y_, cv::INTER_LINEAR, border, 0.8);
auto t =
timers_planar_renderer[TimerPlanarRenderer::remap].timeScope();
cv::remap(
texture_,
*out_image,
map_x_,
map_y_,
cv::INTER_LINEAR,
border,
0.8
);
}
// Compute depth map in dst
{
auto t = timers_planar_renderer[TimerPlanarRenderer::compute_depthmap].timeScope();
auto t =
timers_planar_renderer[TimerPlanarRenderer::compute_depthmap]
.timeScope();
Vector3 X;
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
X = bearings_C_.col(x + camera_->width() * y);
// @TODO: derive this formula for the depth to explain it
const FloatType numer = -T_P_C.getPosition()[2];
const FloatType denom = T_P_C.getRotationMatrix().row(2) * X;
const FloatType denom =
T_P_C.getRotationMatrix().row(2) * X;
const FloatType z = numer / denom;
if(out_depthmap)
{
(*out_depthmap)(y,x) = (ImageFloatType) z;
}
if (out_depthmap)
(*out_depthmap)(y, x) = (ImageFloatType) z;
// clip depth to a reasonable depth range
if(z < z_min_ || z > z_max_)
{
(*out_image)(y,x) = 0;
if (z < z_min_ || z > z_max_)
(*out_image)(y, x) = 0;
}
}
}
}
}
} // namespace event_camera_simulator

4
event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp
View File

@ -1,5 +1,3 @@
#include <esim/rendering/renderer_base.hpp>
namespace event_camera_simulator {
} // namespace event_camera_simulator
namespace event_camera_simulator {} // namespace event_camera_simulator

231
event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp
View File

@ -1,164 +1,235 @@
#include <esim/common/utils.hpp>
#include <esim/rendering/renderer_factory.hpp>
#include <esim/rendering/planar_renderer.hpp>
#include <esim/rendering/panorama_renderer.hpp>
#include <ze/cameras/camera_models.hpp>
#include <ze/cameras/camera_impl.hpp>
#include <ze/common/logging.hpp>
#include <esim/rendering/planar_renderer.hpp>
#include <esim/rendering/renderer_factory.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/cameras/camera_impl.hpp>
#include <ze/cameras/camera_models.hpp>
#include <ze/common/logging.hpp>
DEFINE_int32(renderer_type, 0, " 0: Planar renderer");
DEFINE_string(renderer_texture, "",
"Path to image which will be used to texture the plane");
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_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_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_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_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_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_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_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_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_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_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_min, 0.01, "Minimum clipping distance.");
DEFINE_double(renderer_z_max, 10.0,
"Maximum 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_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_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_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_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)");
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)
{
bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img) {
CHECK(img);
VLOG(1) << "Loading texture file from file: " << FLAGS_renderer_texture << ".";
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 << ".";
if (!img->data) {
LOG(FATAL) << "Could not open image at: " << FLAGS_renderer_texture
<< ".";
return false;
}
if(FLAGS_renderer_preprocess_median_blur > 1)
{
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: "
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(15,15), FLAGS_renderer_preprocess_gaussian_blur);
cv::GaussianBlur(
*img,
*img,
cv::Size(15, 15),
FLAGS_renderer_preprocess_gaussian_blur
);
}
img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0/255.0);
img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0 / 255.0);
return true;
}
}
Renderer::Ptr loadRendererFromGflags()
{
Renderer::Ptr loadRendererFromGflags() {
Renderer::Ptr renderer;
switch (FLAGS_renderer_type)
{
switch (FLAGS_renderer_type) {
case 0: // Planar renderer
{
cv::Mat img_src;
if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
{
if (!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
return nullptr;
}
double f_src = hfovToFocalLength(FLAGS_renderer_hfov_cam_source_deg, img_src.cols);
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());
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,
T_W_P.getPosition() = ze::Position(
FLAGS_renderer_plane_x,
FLAGS_renderer_plane_y,
FLAGS_renderer_plane_z);
FLAGS_renderer_plane_z
);
T_W_P.getRotation() = ze::Quaternion(FLAGS_renderer_plane_qw,
T_W_P.getRotation() = ze::Quaternion(
FLAGS_renderer_plane_qw,
FLAGS_renderer_plane_qx,
FLAGS_renderer_plane_qy,
FLAGS_renderer_plane_qz);
FLAGS_renderer_plane_qz
);
renderer.reset(new PlanarRenderer(img_src, cam_src,
renderer.reset(new PlanarRenderer(
img_src,
cam_src,
T_W_P,
FLAGS_renderer_z_min,
FLAGS_renderer_z_max,
FLAGS_renderer_extend_border));
FLAGS_renderer_extend_border
));
break;
}
case 1: // Panorama renderer
{
cv::Mat img_src;
if(!loadPreprocessedImage(FLAGS_renderer_texture, &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,
R_W_P = ze::Quaternion(
FLAGS_renderer_panorama_qw,
FLAGS_renderer_panorama_qx,
FLAGS_renderer_panorama_qy,
FLAGS_renderer_panorama_qz);
FLAGS_renderer_panorama_qz
);
renderer.reset(new PanoramaRenderer(img_src, R_W_P));
break;
}
default:
{
default: {
LOG(FATAL) << "Renderer type is not known.";
break;
}
}
return renderer;
}
}
} // namespace event_camera_simulator

36
event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/unrealcv_renderer.hpp
View File

@ -4,31 +4,39 @@
namespace event_camera_simulator {
class UnrealCvClient; // fwd
class UnrealCvRenderer : public Renderer
{
public:
class UnrealCvClient; // fwd
class UnrealCvRenderer : public Renderer {
public:
UnrealCvRenderer();
//! Render image and depth map for a given camera pose
virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const;
virtual void render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const;
void render(const Transformation& T_W_C, const std::vector<Transformation>& T_W_OBJ, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const
{
void render(
const Transformation& T_W_C,
const std::vector<Transformation>& T_W_OBJ,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const {
render(T_W_C, out_image, out_depthmap);
}
//! Returns true if the rendering engine can compute optic flow, false otherwise
virtual bool canComputeOpticFlow() const override { return false; }
//! Returns true if the rendering engine can compute optic flow, false
//! otherwise
virtual bool canComputeOpticFlow() const override {
return false;
}
virtual void setCamera(const ze::Camera::Ptr& camera) override;
private:
private:
std::shared_ptr<UnrealCvClient> client_;
mutable size_t frame_idx_;
};
};
}
} // namespace event_camera_simulator

73
event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/utils.hpp
View File

@ -4,85 +4,86 @@
namespace event_camera_simulator {
/**
/**
* https://github.com/EpicGames/UnrealEngine/blob/dbced2dd59f9f5dfef1d7786fd67ad2970adf95f/Engine/Source/Runtime/Core/Public/Math/Rotator.h#L580
* Helper function for eulerFromQuatSingularityTest, angles are expected to be given in degrees
* Helper function for eulerFromQuatSingularityTest, angles are expected to
*be given in degrees
**/
inline FloatType clampAxis(FloatType angle)
{
inline FloatType clampAxis(FloatType angle) {
// returns angle in the range (-360,360)
angle = std::fmod(angle, 360.f);
if (angle < 0.f)
{
if (angle < 0.f) {
// shift to [0,360) range
angle += 360.f;
}
return angle;
}
}
/**
/**
* https://github.com/EpicGames/UnrealEngine/blob/dbced2dd59f9f5dfef1d7786fd67ad2970adf95f/Engine/Source/Runtime/Core/Public/Math/Rotator.h#L595$
* Helper function for eulerFromQuatSingularityTest, angles are expected to be given in degrees
* Helper function for eulerFromQuatSingularityTest, angles are expected to
*be given in degrees
**/
inline FloatType normalizeAxis(FloatType angle)
{
inline FloatType normalizeAxis(FloatType angle) {
angle = clampAxis(angle);
if(angle > 180.f)
{
if (angle > 180.f) {
// shift to (-180,180]
angle -= 360.f;
}
return angle;
}
}
/**
/**
*
* https://github.com/EpicGames/UnrealEngine/blob/f794321ffcad597c6232bc706304c0c9b4e154b2/Engine/Source/Runtime/Core/Private/Math/UnrealMath.cpp#L540
* Quaternion given in (x,y,z,w) representation
**/
void quaternionToEulerUnrealEngine(const Transformation::Rotation& q, FloatType& yaw, FloatType& pitch, FloatType& roll)
{
void quaternionToEulerUnrealEngine(
const Transformation::Rotation& q,
FloatType& yaw,
FloatType& pitch,
FloatType& roll
) {
const FloatType X = q.x();
const FloatType Y = q.y();
const FloatType Z = q.z();
const FloatType W = q.w();
const FloatType SingularityTest = Z*X-W*Y;
const FloatType YawY = 2.f*(W*Z+X*Y);
const FloatType YawX = (1.f-2.f*(Y*Y + Z*Z));
const FloatType SingularityTest = Z * X - W * Y;
const FloatType YawY = 2.f * (W * Z + X * Y);
const FloatType YawX = (1.f - 2.f * (Y * Y + Z * Z));
// reference
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
// this value was found from experience, the above websites recommend different values
// but that isn't the case for us, so I went through different testing, and finally found the case
// where both of world lives happily.
// this value was found from experience, the above websites recommend
// different values but that isn't the case for us, so I went through
// different testing, and finally found the case where both of world
// lives happily.
const FloatType SINGULARITY_THRESHOLD = 0.4999995;
const FloatType RAD_TO_DEG = (180.0)/CV_PI;
const FloatType RAD_TO_DEG = (180.0) / CV_PI;
if (SingularityTest < -SINGULARITY_THRESHOLD)
{
if (SingularityTest < -SINGULARITY_THRESHOLD) {
pitch = -90.;
yaw = std::atan2(YawY, YawX) * RAD_TO_DEG;
roll = normalizeAxis(-yaw - (2.f * std::atan2(X, W) * RAD_TO_DEG));
}
else if (SingularityTest > SINGULARITY_THRESHOLD)
{
} else if (SingularityTest > SINGULARITY_THRESHOLD) {
pitch = 90.;
yaw = std::atan2(YawY, YawX) * RAD_TO_DEG;
roll = normalizeAxis(yaw - (2.f * std::atan2(X, W) * RAD_TO_DEG));
}
else
{
pitch = std::asin(2.f*(SingularityTest)) * RAD_TO_DEG;
} else {
pitch = std::asin(2.f * (SingularityTest)) * RAD_TO_DEG;
yaw = std::atan2(YawY, YawX) * RAD_TO_DEG;
roll = std::atan2(-2.f*(W*X+Y*Z), (1.f-2.f*(X*X + Y*Y))) * RAD_TO_DEG;
roll = std::atan2(
-2.f * (W * X + Y * Z),
(1.f - 2.f * (X * X + Y * Y))
)
* RAD_TO_DEG;
}
}
}
} // namespace event_camera_simulator

185
event_camera_simulator/imp/imp_unrealcv_renderer/src/unrealcv_renderer.cpp
View File

@ -1,10 +1,10 @@
#include <esim/imp_unrealcv_renderer/unrealcv_renderer.hpp>
#include <esim/unrealcv_bridge/unrealcv_bridge.hpp>
#include <esim/imp_unrealcv_renderer/utils.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ze/common/path_utils.hpp>
#include <esim/unrealcv_bridge/unrealcv_bridge.hpp>
#include <iomanip>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ze/common/path_utils.hpp>
DEFINE_double(x_offset, 0, "X offset of the trajectory, in meters");
DEFINE_double(y_offset, 0, "Y offset of the trajectory, in meters");
@ -13,71 +13,81 @@ DECLARE_double(trajectory_multiplier_x);
DECLARE_double(trajectory_multiplier_y);
DECLARE_double(trajectory_multiplier_z);
DEFINE_int32(unrealcv_post_median_blur, 0,
DEFINE_int32(
unrealcv_post_median_blur,
0,
"If > 0, median blur the raw UnrealCV images"
"with a median filter of this size");
"with a median filter of this size"
);
DEFINE_double(unrealcv_post_gaussian_blur_sigma, 0,
DEFINE_double(
unrealcv_post_gaussian_blur_sigma,
0,
"If sigma > 0, Gaussian blur the raw UnrealCV images"
"with a Gaussian filter standard deviation sigma.");
"with a Gaussian filter standard deviation sigma."
);
DEFINE_string(unrealcv_output_directory, "",
"Output directory in which to output the raw RGB images.");
DEFINE_string(
unrealcv_output_directory,
"",
"Output directory in which to output the raw RGB images."
);
namespace event_camera_simulator {
UnrealCvRenderer::UnrealCvRenderer()
{
UnrealCvRenderer::UnrealCvRenderer() {
client_ = std::make_shared<UnrealCvClient>("localhost", "9000");
frame_idx_ = 0;
}
}
void UnrealCvRenderer::setCamera(const ze::Camera::Ptr& camera)
{
void UnrealCvRenderer::setCamera(const ze::Camera::Ptr& camera) {
camera_ = camera;
// compute the horizontal field of view of the camera
ze::VectorX intrinsics = camera_->projectionParameters();
const FloatType fx = intrinsics(0);
const FloatType hfov_deg = 2 * std::atan(0.5 * (FloatType) camera_->width() / fx) * 180. / CV_PI;
const FloatType hfov_deg =
2 * std::atan(0.5 * (FloatType) camera_->width() / fx) * 180.
/ CV_PI;
client_->setCameraFOV(static_cast<float>(hfov_deg));
client_->setCameraSize(camera->width(), camera->height());
}
}
void UnrealCvRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
{
void UnrealCvRenderer::render(
const Transformation& T_W_C,
const ImagePtr& out_image,
const DepthmapPtr& out_depthmap
) const {
CHECK_EQ(out_image->rows, camera_->height());
CHECK_EQ(out_image->cols, camera_->width());
VLOG(1) << "T_W_C (ZE) = " << T_W_C;
const Transformation::TransformationMatrix mT_ZE_C = T_W_C.getTransformationMatrix();
const Transformation::TransformationMatrix mT_ZE_C =
T_W_C.getTransformationMatrix();
Transformation::TransformationMatrix mT_UE_ZE;
mT_UE_ZE << 1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
mT_UE_ZE << 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
Transformation::TransformationMatrix mT_C_UEC;
mT_C_UEC << 0, 1, 0, 0,
0, 0, -1, 0,
1, 0, 0, 0,
0, 0, 0, 1;
mT_C_UEC << 0, 1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 1;
// rotate 90 deg to the right so that R_W_C = Identity <=> euler angle (0,0,0) in UnrealCV
// Transformation::TransformationMatrix mT_rotate_90_right;
// mT_rotate_90_right << 0, -1, 0, 0,
// 1, 0, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
// mT_rotate_90_right << 1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
// const Transformation::TransformationMatrix mT_UE_UEC = mT_rotate_90_right * mT_UE_ZE * mT_ZE_C * mT_C_UEC;
const Transformation::TransformationMatrix mT_UE_UEC = mT_UE_ZE * mT_ZE_C * mT_C_UEC;
// rotate 90 deg to the right so that R_W_C = Identity <=> euler angle
// (0,0,0) in UnrealCV
// Transformation::TransformationMatrix mT_rotate_90_right;
// mT_rotate_90_right << 0, -1, 0, 0,
// 1, 0, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
// mT_rotate_90_right << 1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
// const Transformation::TransformationMatrix mT_UE_UEC =
// mT_rotate_90_right
// * mT_UE_ZE * mT_ZE_C * mT_C_UEC;
const Transformation::TransformationMatrix mT_UE_UEC =
mT_UE_ZE * mT_ZE_C * mT_C_UEC;
const Transformation T_UE_UEC(mT_UE_UEC);
VLOG(1) << "T_ZE_C = " << mT_ZE_C;
@ -90,30 +100,35 @@ void UnrealCvRenderer::render(const Transformation& T_W_C, const ImagePtr& out_i
const FloatType y_offset = static_cast<FloatType>(FLAGS_y_offset);
const FloatType z_offset = static_cast<FloatType>(FLAGS_z_offset);
const FloatType x = 100. * (FLAGS_trajectory_multiplier_x * T_UE_UEC.getPosition()[0] + x_offset);
const FloatType y = 100. * (FLAGS_trajectory_multiplier_y * T_UE_UEC.getPosition()[1] + y_offset);
const FloatType z = 100. * (FLAGS_trajectory_multiplier_z * T_UE_UEC.getPosition()[2] + z_offset);
const FloatType x =
100.
* (FLAGS_trajectory_multiplier_x * T_UE_UEC.getPosition()[0]
+ x_offset);
const FloatType y =
100.
* (FLAGS_trajectory_multiplier_y * T_UE_UEC.getPosition()[1]
+ y_offset);
const FloatType z =
100.
* (FLAGS_trajectory_multiplier_z * T_UE_UEC.getPosition()[2]
+ z_offset);
VLOG(1) << yaw << " " << pitch << " " << roll;
CameraData cam_data = {0,
pitch,
yaw,
roll,
x,
y,
z};
CameraData cam_data = {0, pitch, yaw, roll, x, y, z};
client_->setCamera(cam_data);
cv::Mat img = client_->getImage(0);
VLOG(5) << "Got image from the UnrealCV client";
// (optionally) save raw RGB image to the output directory
if(FLAGS_unrealcv_output_directory != "")
{
if (FLAGS_unrealcv_output_directory != "") {
std::stringstream ss_nr;
ss_nr << std::setw(10) << std::setfill('0') << frame_idx_;
std::string path_frame = ze::joinPath(FLAGS_unrealcv_output_directory, "frame_" + ss_nr.str() + ".png");
std::string path_frame = ze::joinPath(
FLAGS_unrealcv_output_directory,
"frame_" + ss_nr.str() + ".png"
);
VLOG(1) << "Saving raw RGB image to: " << path_frame;
cv::imwrite(path_frame, img, {cv::IMWRITE_PNG_COMPRESSION, 9});
}
@ -121,18 +136,28 @@ void UnrealCvRenderer::render(const Transformation& T_W_C, const ImagePtr& out_i
cv::Mat img_gray;
cv::cvtColor(img, img_gray, cv::COLOR_BGR2GRAY);
if(FLAGS_unrealcv_post_median_blur > 0)
{
if (FLAGS_unrealcv_post_median_blur > 0)
cv::medianBlur(img_gray, img_gray, FLAGS_unrealcv_post_median_blur);
if (FLAGS_unrealcv_post_gaussian_blur_sigma > 0) {
cv::GaussianBlur(
img_gray,
img_gray,
cv::Size(-1, -1),
FLAGS_unrealcv_post_gaussian_blur_sigma
);
}
if(FLAGS_unrealcv_post_gaussian_blur_sigma > 0)
{
cv::GaussianBlur(img_gray, img_gray, cv::Size(-1,-1), FLAGS_unrealcv_post_gaussian_blur_sigma);
}
cv::resize(img_gray, img_gray, cv::Size(camera_->width(), camera_->height()));
img_gray.convertTo(*out_image, cv::DataType<ImageFloatType>::type, 1./255.);
cv::resize(
img_gray,
img_gray,
cv::Size(camera_->width(), camera_->height())
);
img_gray.convertTo(
*out_image,
cv::DataType<ImageFloatType>::type,
1. / 255.
);
cv::Mat depth = client_->getDepth(0);
VLOG(5) << "Got depth map from the UnrealCV client";
@ -141,25 +166,31 @@ void UnrealCvRenderer::render(const Transformation& T_W_C, const ImagePtr& out_i
cv::resize(depth, depth, cv::Size(camera_->width(), camera_->height()));
depth.convertTo(*out_depthmap, cv::DataType<ImageFloatType>::type);
// the depth provided by the unrealcv client is the distance from the scene to the camera center,
// we need to convert it to the distance to image plane (see Github issue: https://github.com/unrealcv/unrealcv/issues/14)
const ImageFloatType yc = 0.5 * static_cast<ImageFloatType>(camera_->height()) - 1.0;
const ImageFloatType xc = 0.5 * static_cast<ImageFloatType>(camera_->width()) - 1.0;
const ImageFloatType f = static_cast<ImageFloatType>(camera_->projectionParameters()(0));
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
const ImageFloatType point_depth = (*out_depthmap)(y,x);
// the depth provided by the unrealcv client is the distance from the
// scene to the camera center, we need to convert it to the distance to
// image plane (see Github issue:
// https://github.com/unrealcv/unrealcv/issues/14)
const ImageFloatType yc =
0.5 * static_cast<ImageFloatType>(camera_->height()) - 1.0;
const ImageFloatType xc =
0.5 * static_cast<ImageFloatType>(camera_->width()) - 1.0;
const ImageFloatType f =
static_cast<ImageFloatType>(camera_->projectionParameters()(0));
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const ImageFloatType point_depth = (*out_depthmap)(y, x);
const ImageFloatType dx = static_cast<ImageFloatType>(x) - xc;
const ImageFloatType dy = static_cast<ImageFloatType>(y) - yc;
const ImageFloatType distance_from_center = std::sqrt(dx*dx + dy*dy);
const ImageFloatType plane_depth = point_depth / std::sqrt(1.0 + std::pow(distance_from_center / f, 2));
(*out_depthmap)(y,x) = plane_depth;
const ImageFloatType distance_from_center =
std::sqrt(dx * dx + dy * dy);
const ImageFloatType plane_depth =
point_depth
/ std::sqrt(1.0 + std::pow(distance_from_center / f, 2));
(*out_depthmap)(y, x) = plane_depth;
}
}
frame_idx_++;
}
}
} // namespace event_camera_simulator