karma/hdr_esim
1
1
Fork 0
mirror of https://github.com/karma-riuk/hdr_esim.git synced 2025-06-20 11:18:12 +02: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
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,160 +1,155 @@
#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
{
Event(uint16_t x, uint16_t y, Time t, bool pol)
: x(x),
y(y),
t(t),
pol(pol)
{
struct Event {
Event(uint16_t x, uint16_t y, Time t, bool pol)
: x(x),
y(y),
t(t),
pol(pol) {}
}
uint16_t x;
uint16_t y;
Time t;
bool pol;
};
uint16_t x;
uint16_t y;
Time t;
bool pol;
};
using Events = std::vector<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
)
: xyz(x, y, z),
rgb(red, green, blue) {}
struct PointXYZRGB
{
PointXYZRGB(FloatType x, FloatType y, FloatType z,
int red, int green, int blue)
: xyz(x, y, z),
rgb(red, green, blue) {}
PointXYZRGB(const Vector3& xyz): xyz(xyz) {}
PointXYZRGB(const Vector3& xyz)
: xyz(xyz) {}
PointXYZRGB(const Vector3& xyz, const Vector3i& rgb)
: xyz(xyz),
rgb(rgb) {}
PointXYZRGB(const Vector3& xyz, const Vector3i& rgb)
: xyz(xyz),
rgb(rgb) {}
Vector3 xyz;
Vector3i rgb;
};
Vector3 xyz;
Vector3i rgb;
};
using PointCloud = std::vector<PointXYZRGB>;
using PointCloudVector = std::vector<PointCloud>;
using PointCloud = std::vector<PointXYZRGB>;
using PointCloudVector = std::vector<PointCloud>;
struct SimulatorData {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
struct SimulatorData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//! Nanosecond timestamp.
Time timestamp;
//! Nanosecond timestamp.
Time timestamp;
//! Camera images.
ImagePtrVector images;
//! Camera images.
ImagePtrVector images;
//! Depth maps.
DepthmapPtrVector depthmaps;
//! Depth maps.
DepthmapPtrVector depthmaps;
//! Optic flow maps.
OpticFlowPtrVector optic_flows;
//! Optic flow maps.
OpticFlowPtrVector optic_flows;
//! Camera
ze::CameraRig::Ptr camera_rig;
//! Camera
ze::CameraRig::Ptr camera_rig;
//! An accelerometer measures the specific force (incl. gravity),
//! corrupted by noise and bias.
Vector3 specific_force_corrupted;
//! An accelerometer measures the specific force (incl. gravity),
//! corrupted by noise and bias.
Vector3 specific_force_corrupted;
//! The angular velocity (in the body frame) corrupted by noise and
//! bias.
Vector3 angular_velocity_corrupted;
//! The angular velocity (in the body frame) corrupted by noise and bias.
Vector3 angular_velocity_corrupted;
//! Groundtruth states.
struct Groundtruth {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//! Groundtruth states.
struct Groundtruth
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//! Pose of the body (i.e. the IMU) expressed in the world frame.
Transformation T_W_B;
//! Pose of the body (i.e. the IMU) expressed in the world frame.
Transformation T_W_B;
//! Accelerometer and gyro bias
Vector3 acc_bias;
Vector3 gyr_bias;
//! Accelerometer and gyro bias
Vector3 acc_bias;
Vector3 gyr_bias;
//! Poses of the cameras in the rig expressed in the world frame.
TransformationVector T_W_Cs;
//! Poses of the cameras in the rig expressed in the world frame.
TransformationVector T_W_Cs;
//! Linear and angular velocities (i.e. twists) of the cameras in
//! the rig, expressed in each camera's local coordinate frame.
LinearVelocityVector linear_velocities_;
AngularVelocityVector angular_velocities_;
//! Linear and angular velocities (i.e. twists) of the cameras in the rig,
//! expressed in each camera's local coordinate frame.
LinearVelocityVector linear_velocities_;
AngularVelocityVector angular_velocities_;
// dynamic objects
std::vector<Transformation> T_W_OBJ_;
std::vector<LinearVelocity> linear_velocity_obj_;
std::vector<AngularVelocity> angular_velocity_obj_;
};
// dynamic objects
std::vector<Transformation> T_W_OBJ_;
std::vector<LinearVelocity> linear_velocity_obj_;
std::vector<AngularVelocity> angular_velocity_obj_;
};
Groundtruth groundtruth;
Groundtruth groundtruth;
// Flags to indicate whether a value has been updated or not
bool images_updated;
bool depthmaps_updated;
bool optic_flows_updated;
bool twists_updated;
bool poses_updated;
bool imu_updated;
};
// Flags to indicate whether a value has been updated or not
bool images_updated;
bool depthmaps_updated;
bool optic_flows_updated;
bool twists_updated;
bool poses_updated;
bool imu_updated;
};
} // namespace event_camera_simulator

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

@ -3,57 +3,70 @@
#include <esim/common/types.hpp>
namespace ze {
class Camera;
class Camera;
}
namespace event_camera_simulator {
inline double degToRad(double deg)
{
return deg * CV_PI / 180.0;
}
inline double degToRad(double deg) {
return deg * CV_PI / 180.0;
}
inline double hfovToFocalLength(double hfov_deg, int W)
{
return 0.5 * static_cast<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:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Helper class to compute optic flow from a twist vector and depth map
// Precomputes a lookup table for pixel -> bearing vector correspondences
// to accelerate the computation
class OpticFlowHelper {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ZE_POINTER_TYPEDEFS(OpticFlowHelper);
ZE_POINTER_TYPEDEFS(OpticFlowHelper);
OpticFlowHelper(const ze::Camera::Ptr& camera);
OpticFlowHelper(const ze::Camera::Ptr& camera);
void computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC,
const DepthmapPtr& depthmap, OpticFlowPtr& flow);
void computeFromDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
OpticFlowPtr& flow
);
void computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow);
void computeFromDepthCamTwistAndObjDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ,
const ze::Vector3& w_WOBJ,
const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow
);
private:
private:
void precomputePixelToBearingLookupTable();
void precomputePixelToBearingLookupTable();
ze::Camera::Ptr camera_;
ze::Camera::Ptr camera_;
// Precomputed lookup table from keypoint -> bearing vector
ze::Bearings bearings_C_;
};
// Precomputed lookup table from keypoint -> bearing vector
ze::Bearings bearings_C_;
};
} // namespace event_camera_simulator

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

@ -1,190 +1,212 @@
#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)
{
CHECK(camera_);
precomputePixelToBearingLookupTable();
}
void OpticFlowHelper::precomputePixelToBearingLookupTable()
{
// points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for(int y=0; y<camera_->height(); ++y)
{
for(int x=0; x<camera_->width(); ++x)
{
points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
OpticFlowHelper::OpticFlowHelper(const ze::Camera::Ptr& camera)
: camera_(camera) {
CHECK(camera_);
precomputePixelToBearingLookupTable();
}
}
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
void OpticFlowHelper::computeFromDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC,
const DepthmapPtr& depthmap, OpticFlowPtr& flow)
{
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
CHECK(depthmap->isContinuous());
const ze::Vector3 w_CW = -w_WC; // rotation speed of the world wrt the camera
const ze::Vector3 v_CW = -v_WC; // speed of the world wrt the camera
const ze::Matrix33 R_CW = ze::skewSymmetric(w_CW);
Eigen::Map<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);
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;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
void OpticFlowHelper::precomputePixelToBearingLookupTable() {
// points_C is a matrix containing the coordinates of each pixel
// coordinate in the image plane
ze::Keypoints points_C(2, camera_->width() * camera_->height());
for (int y = 0; y < camera_->height(); ++y)
for (int x = 0; x < camera_->width(); ++x)
points_C.col(x + camera_->width() * y) = ze::Keypoint(x, y);
bearings_C_ = camera_->backProjectVectorized(points_C);
bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
}
}
}
void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(const ze::Vector3& w_WC, const ze::Vector3& v_WC, const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ, const ze::Vector3& w_WOBJ, const ze::Vector3& v_WOBJ, OpticFlowPtr& flow)
{
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
CHECK(depthmap->isContinuous());
void OpticFlowHelper::computeFromDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
OpticFlowPtr& flow
) {
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
CHECK(depthmap->isContinuous());
const ze::Matrix33 w_WC_tilde = ze::skewSymmetric(w_WC);
const ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(w_WOBJ);
const ze::Vector3 w_CW =
-w_WC; // rotation speed of the world wrt the camera
const ze::Vector3 v_CW = -v_WC; // speed of the world wrt the camera
const ze::Matrix33 R_CW = ze::skewSymmetric(w_CW);
Eigen::Map<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();
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)
{
const Vector3 r_CX = Xs.col(x + camera_->width() * y);
const Vector3 r_OBJX = r_CX - r_COBJ;
for (int y = 0; y < camera_->height(); ++y) {
for (int x = 0; x < camera_->width(); ++x) {
const Vector3 X = Xs.col(x + camera_->width() * y);
ze::Matrix31 dXdt = R_CW * X + v_CW;
ze::Vector2 flow_vec =
Eigen::Map<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)
{
CHECK(flow);
FloatType max_squared_magnitude = 0;
for(int y=0; y<flow->rows; ++y)
{
for(int x=0; x<flow->cols; ++x)
{
const FloatType squared_magnitude = cv::norm((*flow)(y,x), cv::NORM_L2SQR);
if(squared_magnitude > max_squared_magnitude)
{
max_squared_magnitude = squared_magnitude;
}
void OpticFlowHelper::computeFromDepthCamTwistAndObjDepthAndTwist(
const ze::Vector3& w_WC,
const ze::Vector3& v_WC,
const DepthmapPtr& depthmap,
const ze::Vector3& r_COBJ,
const ze::Vector3& w_WOBJ,
const ze::Vector3& v_WOBJ,
OpticFlowPtr& flow
) {
CHECK(depthmap);
CHECK_EQ(depthmap->rows, camera_->height());
CHECK_EQ(depthmap->cols, camera_->width());
CHECK(depthmap->isContinuous());
const ze::Matrix33 w_WC_tilde = ze::skewSymmetric(w_WC);
const ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(w_WOBJ);
Eigen::Map<
const Eigen::
Matrix<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);
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;
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
return std::sqrt(max_squared_magnitude);
}
FloatType maxPredictedAbsBrightnessChange(const ImagePtr& I, const OpticFlowPtr& flow)
{
const size_t height = I->rows;
const size_t width = I->cols;
Image Ix, Iy; // horizontal/vertical gradients of I
// the factor 1/8 accounts for the scaling introduced by the Sobel filter mask
//cv::Sobel(*I, Ix, cv::DataType<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.);
Image Lx, Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I * dI/dx
static const ImageFloatType eps = 1e-3; // small additive term to avoid problems at I=0
cv::divide(Ix, *I+eps, Lx);
cv::divide(Iy, *I+eps, Ly);
Image dLdt(height, width);
for(int y=0; y<height; ++y)
{
for(int x=0; x<width; ++x)
{
// dL/dt ~= - <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);
FloatType maxMagnitudeOpticFlow(const OpticFlowPtr& flow) {
CHECK(flow);
FloatType max_squared_magnitude = 0;
for (int y = 0; y < flow->rows; ++y) {
for (int x = 0; x < flow->cols; ++x) {
const FloatType squared_magnitude =
cv::norm((*flow)(y, x), cv::NORM_L2SQR);
if (squared_magnitude > max_squared_magnitude)
max_squared_magnitude = squared_magnitude;
}
}
return std::sqrt(max_squared_magnitude);
}
}
double min_dLdt, max_dLdt;
int min_idx, max_idx;
cv::minMaxIdx(dLdt, &min_dLdt, &max_dLdt,
&min_idx, &max_idx);
return static_cast<FloatType>(max_dLdt);
}
FloatType maxPredictedAbsBrightnessChange(
const ImagePtr& I, const OpticFlowPtr& flow
) {
const size_t height = I->rows;
const size_t width = I->cols;
void gaussianBlur(ImagePtr& I, FloatType sigma)
{
cv::GaussianBlur(*I, *I, cv::Size(15,15), sigma, sigma);
}
Image Ix, Iy; // horizontal/vertical gradients of I
// the factor 1/8 accounts for the scaling introduced by the Sobel
// filter mask cv::Sobel(*I, Ix, cv::DataType<ImageFloatType>::type, 1,
// 0, 3, 1./8.); cv::Sobel(*I, Iy, cv::DataType<ImageFloatType>::type,
// 0, 1, 3, 1./8.);
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera)
{
CHECK(camera);
const ze::VectorX params = camera->projectionParameters();
CalibrationMatrix K;
K << params(0), 0, params(2),
0, params(1), params(3),
0, 0, 1;
return K;
}
// the factor 1/32 accounts for the scaling introduced by the Scharr
// filter mask
cv::Scharr(*I, Ix, cv::DataType<ImageFloatType>::type, 1, 0, 1. / 32.);
cv::Scharr(*I, Iy, cv::DataType<ImageFloatType>::type, 0, 1, 1. / 32.);
PointCloud eventsToPointCloud(const Events& events, const Depthmap& depthmap, const ze::Camera::Ptr& camera)
{
PointCloud pcl_camera;
for(const Event& ev : events)
{
Vector3 X_c = camera->backProject(ze::Keypoint(ev.x,ev.y));
X_c[0] /= X_c[2];
X_c[1] /= X_c[2];
X_c[2] = 1.;
const ImageFloatType z = depthmap(ev.y,ev.x);
Vector3 P_c = z * X_c;
Vector3i rgb;
static const Vector3i red(255, 0, 0);
static const Vector3i blue(0, 0, 255);
rgb = (ev.pol) ? blue : red;
PointXYZRGB P_c_intensity(P_c, rgb);
pcl_camera.push_back(P_c_intensity);
}
return pcl_camera;
}
Image Lx,
Ly; // horizontal/vertical gradients of log(I). d(logI)/dx = 1/I *
// dI/dx
static const ImageFloatType eps =
1e-3; // small additive term to avoid problems at I=0
cv::divide(Ix, *I + eps, Lx);
cv::divide(Iy, *I + eps, Ly);
Image dLdt(height, width);
for (int y = 0; y < height; ++y) {
for (int x = 0; x < width; ++x) {
// dL/dt ~= - <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);
}
}
double min_dLdt, max_dLdt;
int 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);
}
CalibrationMatrix calibrationMatrixFromCamera(const Camera::Ptr& camera) {
CHECK(camera);
const ze::VectorX params = camera->projectionParameters();
CalibrationMatrix K;
K << params(0), 0, params(2), 0, params(1), params(3), 0, 0, 1;
return K;
}
PointCloud eventsToPointCloud(
const Events& events,
const Depthmap& depthmap,
const ze::Camera::Ptr& camera
) {
PointCloud pcl_camera;
for (const Event& ev : events) {
Vector3 X_c = camera->backProject(ze::Keypoint(ev.x, ev.y));
X_c[0] /= X_c[2];
X_c[1] /= X_c[2];
X_c[2] = 1.;
const ImageFloatType z = depthmap(ev.y, ev.x);
Vector3 P_c = z * X_c;
Vector3i rgb;
static const Vector3i red(255, 0, 0);
static const Vector3i blue(0, 0, 255);
rgb = (ev.pol) ? blue : red;
PointXYZRGB P_c_intensity(P_c, rgb);
pcl_camera.push_back(P_c_intensity);
}
return pcl_camera;
}
} // namespace event_camera_simulator

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

@ -1,213 +1,252 @@
#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)
{
using namespace ze;
std::string getTestDataDir(const std::string& dataset_name) {
using namespace ze;
const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH");
CHECK(datapath_dir != nullptr)
<< "Did you download the esim_test_data repository and set "
<< "the ESIM_TEST_DATA_PATH environment variable?";
const char* datapath_dir = std::getenv("ESIM_TEST_DATA_PATH");
CHECK(datapath_dir != nullptr)
<< "Did you download the esim_test_data repository and set "
<< "the ESIM_TEST_DATA_PATH environment variable?";
std::string path(datapath_dir);
CHECK(isDir(path)) << "Folder does not exist: " << path;
path = path + "/data/" + dataset_name;
CHECK(isDir(path)) << "Dataset does not exist: " << path;
return path;
std::string path(datapath_dir);
CHECK(isDir(path)) << "Folder does not exist: " << path;
path = path + "/data/" + dataset_name;
CHECK(isDir(path)) << "Dataset does not exist: " << path;
return path;
}
namespace event_camera_simulator {
void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
OpticFlowPtr& flow)
{
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
void computeOpticFlowFiniteDifference(
const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
OpticFlowPtr& flow
) {
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
const FloatType dt = 0.001;
const FloatType dt = 0.001;
for(int y=0; 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.);
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.);
ze::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = -linear_velocity * dt;
ze::Transformation::Rotation dR =
ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = -linear_velocity * dt;
// Transform Xc(t) to Xc(t+dt)
ze::Transformation T_tdt_t;
T_tdt_t.getRotation() = dR;
T_tdt_t.getPosition() = dT;
VLOG(5) << T_tdt_t;
// Transform Xc(t) to Xc(t+dt)
ze::Transformation T_tdt_t;
T_tdt_t.getRotation() = dR;
T_tdt_t.getPosition() = dT;
VLOG(5) << T_tdt_t;
ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t);
ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t);
// Project Xc(t+dt) in the image plane
ze::Keypoint u_t_dt = camera->project(Xc_t_dt);
VLOG(5) << u_t;
VLOG(5) << u_t_dt;
// Project Xc(t+dt) in the image plane
ze::Keypoint u_t_dt = camera->project(Xc_t_dt);
VLOG(5) << u_t;
VLOG(5) << u_t_dt;
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
}
void computeOpticFlowFiniteDifference(const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
const ze::Vector3& r_COBJ,
const ze::Vector3& angular_velocity_obj,
const ze::Vector3& linear_velocity_obj,
OpticFlowPtr& flow)
{
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
void computeOpticFlowFiniteDifference(
const ze::Camera::Ptr& camera,
const ze::Vector3& angular_velocity,
const ze::Vector3& linear_velocity,
const DepthmapPtr& depth,
const ze::Vector3& r_COBJ,
const ze::Vector3& angular_velocity_obj,
const ze::Vector3& linear_velocity_obj,
OpticFlowPtr& flow
) {
CHECK(flow);
CHECK_EQ(flow->rows, camera->height());
CHECK_EQ(flow->cols, camera->width());
const FloatType dt = 0.001;
const FloatType dt = 0.001;
for(int y=0; 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.);
ze::Position r_OBJX = Xc_t - r_COBJ;
ze::Matrix33 w_WOBJ_tilde = ze::skewSymmetric(angular_velocity_obj);
for (int y = 0; y < flow->rows; ++y) {
for (int x = 0; x < flow->cols; ++x) {
ze::Keypoint u_t(x, y);
ze::Bearing f = camera->backProject(u_t);
const FloatType z = static_cast<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::Transformation::Rotation dR = ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = linear_velocity_obj*dt - linear_velocity * dt + w_WOBJ_tilde*r_OBJX*dt;
ze::Transformation::Rotation dR =
ze::Transformation::Rotation::exp(-angular_velocity * dt);
ze::Transformation::Position dT = linear_velocity_obj * dt
- linear_velocity * dt
+ w_WOBJ_tilde * r_OBJX * dt;
// Transform Xc(t) to Xc(t+dt)
ze::Transformation T_tdt_t;
T_tdt_t.getRotation() = dR;
T_tdt_t.getPosition() = dT;
VLOG(5) << T_tdt_t;
// Transform Xc(t) to Xc(t+dt)
ze::Transformation T_tdt_t;
T_tdt_t.getRotation() = dR;
T_tdt_t.getPosition() = dT;
VLOG(5) << T_tdt_t;
ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t);
ze::Position Xc_t_dt = T_tdt_t.transform(Xc_t);
// Project Xc(t+dt) in the image plane
ze::Keypoint u_t_dt = camera->project(Xc_t_dt);
VLOG(5) << u_t;
VLOG(5) << u_t_dt;
// Project Xc(t+dt) in the image plane
ze::Keypoint u_t_dt = camera->project(Xc_t_dt);
VLOG(5) << u_t;
VLOG(5) << u_t_dt;
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y,x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
ze::Vector2 flow_vec = (u_t_dt - u_t) / dt;
(*flow)(y, x) = cv::Vec<FloatType, 2>(flow_vec(0), flow_vec(1));
}
}
}
}
}
} // event_camera_simulator
} // namespace event_camera_simulator
TEST(Utils, testOpticFlowComputation)
{
using namespace event_camera_simulator;
TEST(Utils, testOpticFlowComputation) {
using namespace event_camera_simulator;
// Load camera calib from folder
const std::string path_to_data_folder = getTestDataDir("camera_calibs");
ze::CameraRig::Ptr camera_rig
= ze::cameraRigFromYaml(ze::joinPath(path_to_data_folder, "pinhole_mono.yaml"));
// Load camera calib from folder
const std::string path_to_data_folder = getTestDataDir("camera_calibs");
ze::CameraRig::Ptr camera_rig = ze::cameraRigFromYaml(
ze::joinPath(path_to_data_folder, "pinhole_mono.yaml")
);
CHECK(camera_rig);
CHECK(camera_rig);
const ze::Camera::Ptr camera = camera_rig->atShared(0);
cv::Size sensor_size(camera->width(), camera->height());
const ze::Camera::Ptr camera = camera_rig->atShared(0);
cv::Size sensor_size(camera->width(), camera->height());
OpticFlowPtr flow_analytic =
std::make_shared<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);
// 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);
}
}
}
// Sample random linear and angular velocity
ze::Vector3 angular_velocity, linear_velocity;
angular_velocity.setRandom();
linear_velocity.setRandom();
// Sample random linear and angular velocity
ze::Vector3 angular_velocity, linear_velocity;
angular_velocity.setRandom();
linear_velocity.setRandom();
LOG(INFO) << "w = " << angular_velocity;
LOG(INFO) << "v = " << linear_velocity;
LOG(INFO) << "w = " << angular_velocity;
LOG(INFO) << "v = " << linear_velocity;
// Compute optic flow on the image plane according
// to the sampled angular/linear velocity
OpticFlowHelper optic_flow_helper(camera);
optic_flow_helper.computeFromDepthAndTwist(angular_velocity, linear_velocity,
depth, flow_analytic);
// Compute optic flow on the image plane according
// to the sampled angular/linear velocity
OpticFlowHelper optic_flow_helper(camera);
optic_flow_helper.computeFromDepthAndTwist(
angular_velocity,
linear_velocity,
depth,
flow_analytic
);
OpticFlowPtr flow_finite_diff =
std::make_shared<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);
// 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
);
}
}
}
/**********************************************/
/* repeat optic flow test for dynamic objects */
/**********************************************/
/**********************************************/
/* repeat optic flow test for dynamic objects */
/**********************************************/
// 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
// 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
ze::Vector3 angular_velocity_obj, linear_velocity_obj;
angular_velocity_obj.setRandom();
linear_velocity_obj.setRandom();
ze::Vector3 angular_velocity_obj, linear_velocity_obj;
angular_velocity_obj.setRandom();
linear_velocity_obj.setRandom();
// 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);
// 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
);
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);
// 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
);
}
}
}
}
ZE_UNITTEST_ENTRYPOINT