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