inline cv::Mat opencv_create_mat_with_image2d(const image2d &image,
command_queue &queue = system::default_queue())
{
BOOST_ASSERT(image.get_context() == queue.get_context());
cv::Mat mat;
image_format format = image.get_format();
const cl_image_format *cl_image_format = format.get_format_ptr();
if(cl_image_format->image_channel_data_type == CL_UNORM_INT8 &&
cl_image_format->image_channel_order == CL_BGRA)
{
mat = cv::Mat(image.height(), image.width(), CV_8UC4);
}
else if(cl_image_format->image_channel_data_type == CL_UNORM_INT16 &&
cl_image_format->image_channel_order == CL_BGRA)
{
mat = cv::Mat(image.height(), image.width(), CV_16UC4);
}
else if(cl_image_format->image_channel_data_type == CL_FLOAT &&
cl_image_format->image_channel_order == CL_INTENSITY)
{
mat = cv::Mat(image.height(), image.width(), CV_32FC1);
}
else
{
mat = cv::Mat(image.height(), image.width(), CV_8UC1);
}
opencv_copy_image_to_mat(image, mat, queue);
return mat;
}
/// Enqueues a command to write data from host memory to \p image.
///
/// \see_opencl_ref{clEnqueueWriteImage}
void enqueue_write_image(const image2d &image,
const size_t origin[2],
const size_t region[2],
size_t input_row_pitch,
const void *host_ptr,
const wait_list &events = wait_list())
{
BOOST_ASSERT(m_queue != 0);
BOOST_ASSERT(image.get_context() == this->get_context());
const size_t origin3[3] = { origin[0], origin[1], size_t(0) };
const size_t region3[3] = { region[0], region[1], size_t(1) };
cl_int ret = clEnqueueWriteImage(
m_queue,
image.get(),
CL_TRUE,
origin3,
region3,
input_row_pitch,
0,
host_ptr,
events.size(),
events.get_event_ptr(),
0
);
if(ret != CL_SUCCESS){
BOOST_THROW_EXCEPTION(opencl_error(ret));
}
}
/// Enqueues a command to copy data from \p src_image to \p dst_buffer.
///
/// \see_opencl_ref{clEnqueueCopyImageToBuffer}
event enqueue_copy_image_to_buffer(const image2d &src_image,
const buffer &dst_buffer,
const size_t src_origin[2],
const size_t region[2],
size_t dst_offset,
const wait_list &events = wait_list())
{
BOOST_ASSERT(m_queue != 0);
BOOST_ASSERT(src_image.get_context() == this->get_context());
BOOST_ASSERT(dst_buffer.get_context() == this->get_context());
const size_t src_origin3[3] = { src_origin[0], src_origin[1], size_t(0) };
const size_t region3[3] = { region[0], region[1], size_t(1) };
event event_;
cl_int ret = clEnqueueCopyImageToBuffer(
m_queue,
src_image.get(),
dst_buffer.get(),
src_origin3,
region3,
dst_offset,
events.size(),
events.get_event_ptr(),
&event_.get()
);
if(ret != CL_SUCCESS){
BOOST_THROW_EXCEPTION(opencl_error(ret));
}
return event_;
}
/// Enqueues a command to fill \p image with \p fill_color.
///
/// \see_opencl_ref{clEnqueueFillImage}
///
/// \opencl_version_warning{1,2}
event enqueue_fill_image(const image2d &image,
const void *fill_color,
const size_t origin[2],
const size_t region[2],
const wait_list &events = wait_list())
{
BOOST_ASSERT(m_queue != 0);
BOOST_ASSERT(image.get_context() == this->get_context());
const size_t origin3[3] = { origin[0], origin[1], size_t(0) };
const size_t region3[3] = { region[0], region[1], size_t(1) };
event event_;
cl_int ret = clEnqueueFillImage(
m_queue,
image.get(),
fill_color,
origin3,
region3,
events.size(),
events.get_event_ptr(),
&event_.get()
);
if(ret != CL_SUCCESS){
BOOST_THROW_EXCEPTION(opencl_error(ret));
}
return event_;
}
int sad_distance(const image2d<F>& i1, const image2d<F>& i2,
vint2 a,
vint2 b,
const int winsize,
const int th)
{
int err = 0.f;
const F* row1 = &i1(a - vint2(winsize/2, winsize / 2));
const F* row2 = &i2(b - vint2(winsize/2, winsize / 2));
for (int r = 0; r < winsize and err <= th; r++)
{
int err2 = 0;
#pragma omp simd
for (int c = 0; c < winsize; c++)
err2 += std::abs((row1[c]) - (row2[c]));
err += err2;
row1 = (const F*)((const char*) row1 + i1.pitch());
row2 = (const F*)((const char*) row2 + i2.pitch());
}
return err;
}
inline void opencv_copy_image_to_mat(const image2d &image,
cv::Mat &mat,
command_queue &queue = system::default_queue())
{
BOOST_ASSERT(mat.isContinuous());
BOOST_ASSERT(image.get_context() == queue.get_context());
queue.enqueue_read_image(image, image.origin(), image.size(), mat.data);
}
inline void opencv_copy_image_to_mat(const image2d &image,
cv::Mat &mat,
command_queue &queue = system::default_queue())
{
BOOST_ASSERT(mat.isContinuous());
BOOST_ASSERT(image.get_context() == queue.get_context());
size_t origin[2] = { 0, 0 };
size_t region[2] = { image.width(), image.height() };
queue.enqueue_read_image(image, origin, region, 0, mat.data);
}
void lucas_kanade(const image2d<V>& i1,
const image2d<V>& i2,
OPTS... opts)
{
auto options = iod::D(opts...);
int niterations = options.get(_niterations, 21);
int winsize = options.get(_winsize, 11);
int nscales = options.get(_nscales, 3);
int min_ev = options.get(_min_ev, 0.0001);
int delta = options.get(_delta, 0.1);
auto prediction = options.get(_prediction,
[] (auto p) { return vfloat2(0.f, 0.f); });
auto flow = options.flow;
auto keypoints = options.keypoints;
typedef std::decay_t<decltype(V() - V())> Gr;
pyramid2d<V> pyramid_prev(i1, nscales, 2, _border = winsize / 2);
pyramid2d<vector<Gr, 2>> pyramid_prev_grad(i1.domain(), nscales, 2, _border = winsize / 2);
pyramid2d<V> pyramid_next(i2, nscales, 2, _border = winsize / 2);
scharr(pyramid_prev[0], pyramid_prev_grad[0]);
pyramid_prev_grad.propagate_level0();
for (int i = 0; i < keypoints.size(); i++)
{
auto kp = keypoints[i];
vfloat2 tr = (prediction(kp)).template cast<float>() / float(std::pow(2, nscales));
float dist = 0.f;
for(int S = pyramid_prev.size() - 1; S >= 0; S--)
{
tr *= pyramid_prev.factor();
auto match = lk_internals::match(kp.template cast<float>() / int(std::pow(2, S)),
tr,
pyramid_prev[S],
pyramid_next[S],
pyramid_prev_grad[S],
winsize,
min_ev,
niterations, delta);
tr = match.first;
dist = match.second;
}
flow(kp, tr, dist);
}
}
/// Enqueues a command to copy data from \p src_image to \p dst_image.
///
/// \see_opencl_ref{clEnqueueCopyImage}
event enqueue_copy_image(const image3d &src_image,
const image2d &dst_image,
const size_t src_origin[3],
const size_t dst_origin[2],
const size_t region[2],
const wait_list &events = wait_list())
{
BOOST_ASSERT(m_queue != 0);
BOOST_ASSERT(src_image.get_context() == this->get_context());
BOOST_ASSERT(dst_image.get_context() == this->get_context());
BOOST_ASSERT_MSG(src_image.get_format() == dst_image.get_format(),
"Source and destination image formats must match.");
const size_t dst_origin3[3] = { dst_origin[0], dst_origin[1], size_t(0) };
const size_t region3[3] = { region[0], region[1], size_t(1) };
event event_;
cl_int ret = clEnqueueCopyImage(
m_queue,
src_image.get(),
dst_image.get(),
src_origin,
dst_origin3,
region3,
events.size(),
events.get_event_ptr(),
&event_.get()
);
if(ret != CL_SUCCESS){
BOOST_THROW_EXCEPTION(opencl_error(ret));
}
return event_;
}
inline void qt_copy_qimage_to_image2d(const QImage &qimage,
image2d &image,
command_queue &queue)
{
queue.enqueue_write_image(image, image.origin(), image.size(), qimage.constBits());
}
bool jpeg_write_uint8_t(std::ostream& stream, const image2d<uint8_t>& imag) {
jpeg_compress_struct
jpeg_ptr{};
jpeg_error_mgr
jerr{};
jpeg_ptr.err = jpeg_std_error(&jerr);
jerr.error_exit = jpeg_exit;
jerr.output_message = jpeg_message;
jpeg_create_compress(&jpeg_ptr);
if (!jpeg_ptr.dest) {
jpeg_ptr.dest = reinterpret_cast<jpeg_destination_mgr*>((*jpeg_ptr.mem->alloc_small)
(reinterpret_cast<j_common_ptr>(&jpeg_ptr), JPOOL_PERMANENT, sizeof(jpeg_dst)));
}
auto dst = reinterpret_cast<jpeg_dst*>(jpeg_ptr.dest);
dst->stream = &stream;
dst->jpeg.empty_output_buffer = &jpeg_writeproc;
dst->jpeg.init_destination = [](j_compress_ptr compress) {
auto dst = reinterpret_cast<jpeg_dst*>(compress->dest);
dst->buffer = reinterpret_cast<JOCTET*>((*compress->mem->alloc_small)
(reinterpret_cast<j_common_ptr>(compress), JPOOL_IMAGE, buffer * sizeof(JOCTET)));
dst->jpeg.next_output_byte = dst->buffer;
dst->jpeg.free_in_buffer = buffer;
};
dst->jpeg.term_destination = [](j_compress_ptr jpeg_ptr) {
auto dst = reinterpret_cast<jpeg_dst*>(jpeg_ptr->dest);
auto count = buffer - dst->jpeg.free_in_buffer;
if (count > 0) {
dst->stream->write(reinterpret_cast<char*>(dst->buffer), count);
}
};
jpeg_ptr.image_width = imag.get_shape()[0], jpeg_ptr.image_height = imag.get_shape()[1];
switch (imag.get_features()) {
case 1:
jpeg_ptr.in_color_space = JCS_GRAYSCALE;
jpeg_ptr.input_components = 1;
break;
case 3:
jpeg_ptr.in_color_space = JCS_RGB;
jpeg_ptr.input_components = 3;
break;
default:
return false;
}
jpeg_set_defaults(&jpeg_ptr);
jpeg_set_quality(&jpeg_ptr, 95, true);
jpeg_start_compress(&jpeg_ptr, true);
while (jpeg_ptr.next_scanline < jpeg_ptr.image_height) {
auto r = imag.buffer() + (imag.get_shape().front() * imag.get_features() * jpeg_ptr.next_scanline);
jpeg_write_scanlines(&jpeg_ptr, (JSAMPARRAY)&r, 1);
}
jpeg_finish_compress(&jpeg_ptr);
jpeg_destroy_compress(&jpeg_ptr);
return true;
}
inline void
semi_dense_optical_flow(const K& keypoints,
MC match_callback,
const image2d<unsigned char>& i1,
const image2d<unsigned char>& i2,
OPTS... options)
{
using Eigen::Matrix3f;
auto opts = D(options...);
const int winsize = opts.get(_winsize, 7);
const int nscales = opts.get(_nscales, 4);
const int min_scale = opts.get(_min_scale, 0);
const int propagation_niters = opts.get(_propagation, 2);
const int patchsize = opts.get(_patchsize, 5);
const bool f_provided = opts.has(_fundamental_matrix);
const Matrix3f F = opts.get(_fundamental_matrix, Matrix3f::Zero());
constexpr bool epipolar_flow = opts.has(_epipolar_flow);
constexpr bool epipolar_filter = opts.has(_epipolar_filter);
const bool epipolar_filter_th = opts.get(_epipolar_filter, 2);
auto pf_domain = make_box2d(i1.domain().nrows() / patchsize,
i1.domain().ncols() / patchsize);
pyramid2d<vint2> pyr_flow_map(pf_domain, nscales, 2, _border = nscales);
pyramid2d<unsigned char> pyr_flow_map_mark(pf_domain, nscales, 2, _border = nscales);
pyramid2d<unsigned char> pyr_i1(i1, nscales, 2, _border = 2 * winsize);
pyramid2d<unsigned char> pyr_i2(i2, nscales, 2, _border = 2 * winsize);
pyramid2d<int> distance_map(pf_domain, nscales, 2, _border = nscales);
assert(pyr_flow_map_mark.size() == nscales);
// Compute epipole for epipolar flow.
vfloat2 epipole = epipole_right(F);
// Scale fundamental matrix.
std::vector<Matrix3f> Fs(nscales, F);
for (int scale = nscales - 1; scale >= 0; scale--)
{
Matrix3f downscale;
downscale <<
2, 2, 1,
2, 2, 1,
1, 1, 0.5;
Fs[scale] = Fs[scale + 1].cwiseProduct(downscale);
}
for (int scale = nscales - 1; scale >= min_scale; scale--)
{
int scale_div= std::pow(2, scale);
image2d<unsigned char> i1 = pyr_i1[scale];
image2d<unsigned char> i2 = pyr_i2[scale];
fill_border_mirror(i1);
fill_border_mirror(i2);
// Distance function
auto distance = [&] (vint2 a, vint2 b, int max_distance)
{
if (i1.has(a) and i2.has(b))
return of_internals::sad_distance(i1, i2, a, b, winsize, max_distance);
else
return INT_MAX;
};
image2d<unsigned char>& flow_map_mark = pyr_flow_map_mark[scale];
fill_with_border(flow_map_mark, 0);
// Gradient descent
#pragma omp parallel for
for (int i = 0; i < keypoints.size(); i++)
{
vint2 p = keypoints[i] / scale_div;
// Descent
if (!pyr_flow_map_mark[scale](p / patchsize))
{
vint2 pf = p / patchsize;
vint2 pfm = p / (2 * patchsize);
vint2 prediction = p;
// Multiscale Prediction
if (scale < (nscales - 1) and pyr_flow_map_mark[scale + 1](pfm))
prediction = p + pyr_flow_map[scale + 1](pfm) * 2;
pyr_flow_map_mark[scale](p / patchsize) = 1;
auto m = iod::static_if<epipolar_flow>
([&] { return epipolar_match(p, prediction, epipole, Fs[scale], distance); },
[&] { return gradient_descent_match(p, prediction, distance, 5); });
// Register match.
vint2 match = p + m.flow;
assert(pyr_flow_map_mark[scale].domain_with_border().has(p / patchsize));
pyr_flow_map[scale](p / patchsize) = match - p;
distance_map[scale](p / patchsize) = m.distance;
pyr_flow_map_mark[scale](p / patchsize) = 2;
}
}
// Propagation
for (int Ki = 0; Ki < propagation_niters; Ki++)
{
auto loop_body = [&] (int r, int c)
{
auto& flow_map = pyr_flow_map[scale];
vint2 p(r, c);
vint2 pf(r / patchsize, c / patchsize);
if (!pyr_flow_map_mark[scale](pf)) return;
vint2 prev_flow = flow_map(pf);
for (int dr = -1; dr <= 1; dr++)
for (int dc = -1; dc <= 1; dc++)
{
if (!dr and !dc) continue;
vint2 pfn(pf[0] + dr, pf[1] + dc);
if (flow_map.has(pfn) and pyr_flow_map_mark[scale](pfn) and
(flow_map(pf) - flow_map(pfn)).norm() > 2 and
(prev_flow - flow_map(pfn)).norm() > 2)
{
// Two neighbors with high divergence.
int d1 = distance_map[scale](pf);
int d2 = distance(p, p + flow_map(pfn), INT_MAX);
if (d2 < d1)
{
auto m = iod::static_if<epipolar_flow>
([&] { return epipolar_match(p, vint2(p + flow_map(pfn)), epipole, Fs[scale], distance); },
[&] { return gradient_descent_match(p, p + flow_map(pfn), distance, 5); });
// Register match.
vint2 match = p + m.flow;
if (m.distance < d1)
{
pyr_flow_map_mark[scale](pf) = true;
pyr_flow_map[scale](pf) = match - p;
distance_map[scale](pf) = m.distance;
}
}
}
}
};
if (Ki % 2)
#pragma omp parallel for
for (int r = 0; r < i1.nrows(); r+= patchsize)
for (int c = 0; c < i1.ncols(); c+= patchsize)
loop_body(r, c);
else
#pragma omp parallel for
for (int r = i1.nrows() - 1; r >= 0; r-= patchsize)
for (int c = i1.ncols() - 1; c >= 0; c-= patchsize)
loop_body(r, c);
}
}
for (int pi = 0; pi < keypoints.size(); pi++)
{
vint2 pos = keypoints[pi];
vint2 pos2 = pos / int(patchsize * std::pow(2, min_scale));
if (pyr_flow_map_mark[min_scale].has(pos2) and pyr_flow_map_mark[min_scale](pos2))
match_callback(pi, pos + pyr_flow_map[min_scale](pos2) * int(std::pow(2, min_scale)),
distance_map[min_scale](pos2));
}
}
