src/complexmat.hpp

   1 #ifndef COMPLEX_MAT_HPP_213123048309482094
   2 #define COMPLEX_MAT_HPP_213123048309482094
   3
   4 #include <opencv2/opencv.hpp>
   5 #include <vector>
   6 #include <algorithm>
   7 #include <functional>
   8 #include "dynmem.hpp"
   9
  10 template <typename T> class ComplexMat_ {
  11   public:
  12     uint cols;
  13     uint rows;
  14     uint n_channels;
  15     uint n_scales;
  16
  17     ComplexMat_() : cols(0), rows(0), n_channels(0), n_scales(0) {}
  18     ComplexMat_(uint _rows, uint _cols, uint _n_channels, uint _n_scales = 1)
  19         : cols(_cols), rows(_rows), n_channels(_n_channels * _n_scales), n_scales(_n_scales)
  20     {
  21         p_data.resize(n_channels * cols * rows);
  22     }
  23     ComplexMat_(cv::Size size, uint _n_channels, uint _n_scales = 1)
  24         : cols(size.width), rows(size.height), n_channels(_n_channels * _n_scales), n_scales(_n_scales)
  25     {
  26         p_data.resize(n_channels * cols * rows);
  27     }
  28
  29     // assuming that mat has 2 channels (real, img)
  30     ComplexMat_(const cv::Mat &mat) : cols(uint(mat.cols)), rows(uint(mat.rows)), n_channels(1), n_scales(1)
  31     {
  32         p_data = convert(mat);
  33     }
  34
  35     void create(uint _rows, uint _cols, uint _n_channels, uint _n_scales = 1)
  36     {
  37         rows = _rows;
  38         cols = _cols;
  39         n_channels = _n_channels * _n_scales;
  40         n_scales = _n_scales;
  41         p_data.resize(n_channels * cols * rows);
  42     }
  43     // cv::Mat API compatibility
  44     cv::Size size() const { return cv::Size(cols, rows); }
  45     uint channels() const { return n_channels; }
  46
  47     // assuming that mat has 2 channels (real, imag)
  48     void set_channel(uint idx, const cv::Mat &mat)
  49     {
  50         assert(idx >= 0 && idx < n_channels);
  51         for (uint i = 0; i < rows; ++i) {
  52             const std::complex<T> *row = mat.ptr<std::complex<T>>(i);
  53             for (uint j = 0; j < cols; ++j)
  54                 p_data[idx * rows * cols + i * cols + j] = row[j];
  55         }
  56     }
  57
  58     T sqr_norm() const
  59     {
  60         assert(n_scales == 1);
  61
  62         int n_channels_per_scale = n_channels / n_scales;
  63         T sum_sqr_norm = 0;
  64         for (int i = 0; i < n_channels_per_scale; ++i) {
  65             for (auto lhs = p_data.begin() + i * rows * cols; lhs != p_data.begin() + (i + 1) * rows * cols; ++lhs)
  66                 sum_sqr_norm += lhs->real() * lhs->real() + lhs->imag() * lhs->imag();
  67         }
  68         sum_sqr_norm = sum_sqr_norm / static_cast<T>(cols * rows);
  69         return sum_sqr_norm;
  70     }
  71
  72     void sqr_norm(DynMem_<T> &result) const
  73     {
  74         int n_channels_per_scale = n_channels / n_scales;
  75         int scale_offset = n_channels_per_scale * rows * cols;
  76         for (uint scale = 0; scale < n_scales; ++scale) {
  77             T sum_sqr_norm = 0;
  78             for (int i = 0; i < n_channels_per_scale; ++i)
  79                 for (auto lhs = p_data.begin() + i * rows * cols + scale * scale_offset;
  80                      lhs != p_data.begin() + (i + 1) * rows * cols + scale * scale_offset; ++lhs)
  81                     sum_sqr_norm += lhs->real() * lhs->real() + lhs->imag() * lhs->imag();
  82             result.hostMem()[scale] = sum_sqr_norm / static_cast<T>(cols * rows);
  83         }
  84         return;
  85     }
  86
  87     ComplexMat_<T> sqr_mag() const
  88     {
  89         return mat_const_operator([](std::complex<T> &c) { c = c.real() * c.real() + c.imag() * c.imag(); });
  90     }
  91
  92     ComplexMat_<T> conj() const
  93     {
  94         return mat_const_operator([](std::complex<T> &c) { c = std::complex<T>(c.real(), -c.imag()); });
  95     }
  96
  97     ComplexMat_<T> sum_over_channels() const
  98     {
  99         assert(p_data.size() == n_channels * rows * cols);
 100
 101         uint n_channels_per_scale = n_channels / n_scales;
 102         uint scale_offset = n_channels_per_scale * rows * cols;
 103
 104         ComplexMat_<T> result(this->rows, this->cols, 1, n_scales);
 105         for (uint scale = 0; scale < n_scales; ++scale) {
 106             for (uint i = 0; i < rows * cols; ++i) {
 107                 std::complex<T> acc = 0;
 108                 for (uint ch = 0; ch < n_channels_per_scale; ++ch)
 109                     acc +=  p_data[scale * scale_offset + i + ch * rows * cols];
 110                 result.p_data[scale * rows * cols + i] = acc;
 111             }
 112         }
 113         return result;
 114     }
 115
 116     // return 2 channels (real, imag) for first complex channel
 117     cv::Mat to_cv_mat() const
 118     {
 119         assert(p_data.size() >= 1);
 120         return channel_to_cv_mat(0);
 121     }
 122     // return a vector of 2 channels (real, imag) per one complex channel
 123     std::vector<cv::Mat> to_cv_mat_vector() const
 124     {
 125         std::vector<cv::Mat> result;
 126         result.reserve(n_channels);
 127
 128         for (uint i = 0; i < n_channels; ++i)
 129             result.push_back(channel_to_cv_mat(i));
 130
 131         return result;
 132     }
 133
 134     std::complex<T> *get_p_data() { return p_data.data(); }
 135     const std::complex<T> *get_p_data() const { return p_data.data(); }
 136
 137     // element-wise per channel multiplication, division and addition
 138     ComplexMat_<T> operator*(const ComplexMat_<T> &rhs) const
 139     {
 140         return mat_mat_operator([](std::complex<T> &c_lhs, const std::complex<T> &c_rhs) { c_lhs *= c_rhs; }, rhs);
 141     }
 142     ComplexMat_<T> operator/(const ComplexMat_<T> &rhs) const
 143     {
 144         return mat_mat_operator([](std::complex<T> &c_lhs, const std::complex<T> &c_rhs) { c_lhs /= c_rhs; }, rhs);
 145     }
 146     ComplexMat_<T> operator+(const ComplexMat_<T> &rhs) const
 147     {
 148         return mat_mat_operator([](std::complex<T> &c_lhs, const std::complex<T> &c_rhs) { c_lhs += c_rhs; }, rhs);
 149     }
 150
 151     // multiplying or adding constant
 152     ComplexMat_<T> operator*(const T &rhs) const
 153     {
 154         return mat_const_operator([&rhs](std::complex<T> &c) { c *= rhs; });
 155     }
 156     ComplexMat_<T> operator+(const T &rhs) const
 157     {
 158         return mat_const_operator([&rhs](std::complex<T> &c) { c += rhs; });
 159     }
 160
 161     // multiplying element-wise multichannel by one channel mats (rhs mat is with one channel)
 162     ComplexMat_<T> mul(const ComplexMat_<T> &rhs) const
 163     {
 164         return matn_mat1_operator([](std::complex<T> &c_lhs, const std::complex<T> &c_rhs) { c_lhs *= c_rhs; }, rhs);
 165     }
 166
 167     // multiplying element-wise multichannel mats - same as operator*(ComplexMat), but without allocating memory for the result
 168     ComplexMat_<T> muln(const ComplexMat_<T> &rhs) const
 169     {
 170         return mat_mat_operator([](std::complex<T> &c_lhs, const std::complex<T> &c_rhs) { c_lhs *= c_rhs; }, rhs);
 171     }
 172
 173     // text output
 174     friend std::ostream &operator<<(std::ostream &os, const ComplexMat_<T> &mat)
 175     {
 176         // for (int i = 0; i < mat.n_channels; ++i){
 177         for (int i = 0; i < 1; ++i) {
 178             os << "Channel " << i << std::endl;
 179             for (uint j = 0; j < mat.rows; ++j) {
 180                 for (uint k = 0; k < mat.cols - 1; ++k)
 181                     os << mat.p_data[j * mat.cols + k] << ", ";
 182                 os << mat.p_data[j * mat.cols + mat.cols - 1] << std::endl;
 183             }
 184         }
 185         return os;
 186     }
 187
 188   private:
 189     std::vector<std::complex<T>> p_data;
 190
 191     // convert 2 channel mat (real, imag) to vector row-by-row
 192     std::vector<std::complex<T>> convert(const cv::Mat &mat)
 193     {
 194         std::vector<std::complex<T>> result;
 195         result.reserve(mat.cols * mat.rows);
 196         for (int y = 0; y < mat.rows; ++y) {
 197             const T *row_ptr = mat.ptr<T>(y);
 198             for (int x = 0; x < 2 * mat.cols; x += 2) {
 199                 result.push_back(std::complex<T>(row_ptr[x], row_ptr[x + 1]));
 200             }
 201         }
 202         return result;
 203     }
 204
 205     ComplexMat_<T> mat_mat_operator(void (*op)(std::complex<T> &c_lhs, const std::complex<T> &c_rhs),
 206                                     const ComplexMat_<T> &mat_rhs) const
 207     {
 208         assert(mat_rhs.n_channels == n_channels/n_scales && mat_rhs.cols == cols && mat_rhs.rows == rows);
 209
 210         ComplexMat_<T> result = *this;
 211         for (uint s = 0; s < n_scales; ++s) {
 212             auto lhs = result.p_data.begin() + (s * n_channels/n_scales * rows * cols);
 213             auto rhs = mat_rhs.p_data.begin();
 214             for (uint i = 0; i < n_channels/n_scales * rows * cols; ++i)
 215                 op(*(lhs + i), *(rhs + i));
 216         }
 217
 218         return result;
 219     }
 220     ComplexMat_<T> matn_mat1_operator(void (*op)(std::complex<T> &c_lhs, const std::complex<T> &c_rhs),
 221                                       const ComplexMat_<T> &mat_rhs) const
 222     {
 223         assert(mat_rhs.n_channels == 1 && mat_rhs.cols == cols && mat_rhs.rows == rows);
 224
 225         ComplexMat_<T> result = *this;
 226         for (uint i = 0; i < n_channels; ++i) {
 227             auto lhs = result.p_data.begin() + i * rows * cols;
 228             auto rhs = mat_rhs.p_data.begin();
 229             for (; lhs != result.p_data.begin() + (i + 1) * rows * cols; ++lhs, ++rhs)
 230                 op(*lhs, *rhs);
 231         }
 232
 233         return result;
 234     }
 235     ComplexMat_<T> matn_mat2_operator(void (*op)(std::complex<T> &c_lhs, const std::complex<T> &c_rhs),
 236                                       const ComplexMat_<T> &mat_rhs) const
 237     {
 238         assert(mat_rhs.n_channels == n_channels / n_scales && mat_rhs.cols == cols && mat_rhs.rows == rows);
 239
 240         int n_channels_per_scale = n_channels / n_scales;
 241         int scale_offset = n_channels_per_scale * rows * cols;
 242         ComplexMat_<T> result = *this;
 243         for (uint i = 0; i < n_scales; ++i) {
 244             for (int j = 0; j < n_channels_per_scale; ++j) {
 245                 auto lhs = result.p_data.begin() + (j * rows * cols) + (i * scale_offset);
 246                 auto rhs = mat_rhs.p_data.begin() + (j * rows * cols);
 247                 for (; lhs != result.p_data.begin() + ((j + 1) * rows * cols) + (i * scale_offset); ++lhs, ++rhs)
 248                     op(*lhs, *rhs);
 249             }
 250         }
 251
 252         return result;
 253     }
 254     ComplexMat_<T> mat_const_operator(const std::function<void(std::complex<T> &c_rhs)> &op) const
 255     {
 256         ComplexMat_<T> result = *this;
 257         for (uint i = 0; i < n_channels; ++i)
 258             for (auto lhs = result.p_data.begin() + i * rows * cols;
 259                  lhs != result.p_data.begin() + (i + 1) * rows * cols; ++lhs)
 260                 op(*lhs);
 261         return result;
 262     }
 263
 264     cv::Mat channel_to_cv_mat(int channel_id) const
 265     {
 266         cv::Mat result(rows, cols, CV_32FC2);
 267         for (uint y = 0; y < rows; ++y) {
 268             std::complex<T> *row_ptr = result.ptr<std::complex<T>>(y);
 269             for (uint x = 0; x < cols; ++x) {
 270                 row_ptr[x] = p_data[channel_id * rows * cols + y * cols + x];
 271             }
 272         }
 273         return result;
 274     }
 275 };
 276
 277 typedef ComplexMat_<float> ComplexMat;
 278
 279 #endif // COMPLEX_MAT_HPP_213123048309482094