////////////////////////////////////////////////////////////////////////////////////////////////////////
//construct hankel matrix
cv::Mat constructHankel(std::vector<cv::Mat> & state, cv::Range rowrange, cv::Range colrange)
{
if (state.size() % 2 == 0)
{
std::cout << "ERROR: state size not odd, in funtion constructHankel:" << state.size() << std::endl;
}
int N = state.size() % 2 ? (state.size() + 1)/2 : state.size()/2;
int nr = rowrange.size();
int nc = colrange.size();
cv::Mat hankel(N*nr, N*nc, CV_32F);
for (int r = 0; r < N; r++)
{
for (int c = 0; c < N; c++)
{
for (int i = 0; i < nr; i++)
{
for (int j = 0; j < nc; j++)
{
//hankel.at<float>(r*nr+i,c*nc+j) = state[r+c].at<float>(rowrange.start+i, colrange.start+j);
hankel.at<float>(i*N+r,j*N+c) = state[r+c].at<float>(rowrange.start+i, colrange.start+j);
}
}
}
}
return hankel;
}
void removeCols(InputArray _src, OutputArray _dst, cv::Range range)
{
CV_Assert( range.start >= 0 && range.end < _src.getMat().cols );
Mat src = _src.getMat();
int type = src.type();
_dst.create( src.rows, src.cols - range.size() - 1, type );
Mat dst = _dst.getMat();
if(src.data != dst.data || src.step != dst.step)
{
for(int i = range.start; i <= range.end; ++i)
{
// cout << i << endl;
removeCol(src, src, range.start);
// cout << "src: " << endl << src << endl;
}
src.copyTo(dst);
}
return;
}