void HarrisDetector::harrisCorners(const cv::Mat& image, std::vector<cv::KeyPoint>& keyPoints, int oct) const
{
cv::Mat blur_img(image.size(), CV_32F);
cv::Mat Ix(image.size(), CV_32F), Iy(image.size(), CV_32F);
cv::Mat Ix2(image.size(), CV_32F), Iy2(image.size(), CV_32F), IxIy(image.size(), CV_32F);
cv::Mat A(image.size(), CV_32F), B(image.size(), CV_32F), C(image.size(), CV_32F);
cv::Mat theta_Ix(image.size(), CV_32F), theta_Iy(image.size(), CV_32F);
cv::GaussianBlur(image, blur_img, cv::Size(harris_blur_size,harris_blur_size), harris_blur_variance, harris_blur_variance, cv::BORDER_DEFAULT );
blur_img.convertTo(blur_img, CV_32F);
cv::Sobel( blur_img, Ix, CV_32F, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT );
cv::Sobel( blur_img, Iy, CV_32F, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT );
cv::Mat kernel_x, kernel_y, sX, sY;
cv::getDerivKernels(sX, sY, 1, 0, harris_window_size, false, CV_32F);
kernel_x = sX * sY.t();
cv::getDerivKernels(sX, sY, 0, 1, harris_window_size, false, CV_32F);
kernel_y = sX * sY.t();
cv::filter2D(blur_img, theta_Ix, -1, kernel_x, cv::Point(-1,-1), 0, cv::BORDER_DEFAULT);
cv::filter2D(blur_img, theta_Iy, -1, kernel_y, cv::Point(-1,-1), 0, cv::BORDER_DEFAULT);
Ix2 = Ix.mul(Ix);
Iy2 = Iy.mul(Iy);
IxIy = Ix.mul(Iy);
float sigma = 0.3*((harris_window_size-1)*0.5-1)+0.8;
cv::Mat gaussKernel = cv::getGaussianKernel(harris_window_size, sigma, CV_32F);
cv::filter2D(Ix2, A, -1, gaussKernel, cv::Point(-1,-1), 0, cv::BORDER_DEFAULT );
cv::filter2D(IxIy, B, -1, gaussKernel, cv::Point(-1,-1), 0, cv::BORDER_DEFAULT );
cv::filter2D(Iy2, C, -1, gaussKernel, cv::Point(-1,-1), 0, cv::BORDER_DEFAULT );
cv::Mat H(image.size(), CV_32F);
H = A.mul(C) - B.mul(B) - 0.04*(A+C).mul(A+C);
for(int i=0;i<H.rows;i++)
{
for(int j=0;j<H.cols;j++)
{
float& Hij = H.at<float>(i,j);
if(Hij>0)
{
float theta = std::atan2(theta_Iy.at<float>(i,j), theta_Ix.at<float>(i,j));
keyPoints.push_back(cv::KeyPoint(cv::Point(j,i),0,theta,Hij,oct));
}
}
}
}
void OpticalFlow::baseCalculate(cv::Mat& Im1, cv::Mat& Im2, flowUV& UV, const OpticalFlowParams& params){
int rows = Im1.rows;
int cols = Im1.cols;
FlowOperator flowOp(rows, cols);
FArray X0(2 * rows * cols, false);
FArray dUdV(2 * rows * cols, true, 0);
cv::Mat Ix1(rows, cols, OPTFLOW_TYPE);
cv::Mat Iy1(rows, cols, OPTFLOW_TYPE);
cv::Mat Ix(rows, cols, OPTFLOW_TYPE);
cv::Mat Iy(rows, cols, OPTFLOW_TYPE);
getDXsCV(Im1, Ix1, Iy1);
for (int i = 0; i < params.getIters(); ++i){
cv::Mat Ix2(rows, cols, OPTFLOW_TYPE);
cv::Mat Iy2(rows, cols, OPTFLOW_TYPE);
cv::Mat It(rows, cols, OPTFLOW_TYPE);
cv::Mat im2Warpped(rows, cols, Im1.type());
WarpImage(Im2, UV.getU(), UV.getV(), im2Warpped);
getDXsCV(im2Warpped, Ix2, Iy2);
Ix = params.getWeightedDeriveFactor() * (Ix1 + Ix2);
Iy = params.getWeightedDeriveFactor() * (Iy1 + Iy2);
cv::subtract(im2Warpped, Im1, It);
if (params.getDisplayDerivativs()){
cv::imshow("Derivative Ix", Ix);
cv::imshow("Derivative Iy", Iy);
cv::waitKey(1);
}
cv::Mat Du(rows, cols, OPTFLOW_TYPE, cv::Scalar(0));
cv::Mat Dv(rows, cols, OPTFLOW_TYPE, cv::Scalar(0));
for (int j = 0; j < params.getLinearIters(); ++j){
#if OPTFLOW_VERBOSE
cout << "solving Ax=b with SOR ";
clock_t start = std::clock();
#endif
flowOp.construct(UV, Du, Dv, Ix, Iy, It, params);
memcpy(X0.ptr, UV.getU().data, rows * cols * sizeof(float));
memcpy(X0.ptr + (rows * cols), UV.getV().data, rows * cols * sizeof(float));
//UtilsDebug::printCRSSparseMat(flowOp.getA(), "aaaa.txt");
if (params.getCheckResidualTolerance()){
LinearSolver::sparseMatSor(flowOp.getA(), X0 ,dUdV, flowOp.getb(), params.getOverRelaxation(), params.getSorIters(), params.getResidualTolerance());
}else{
//LinearSolver::multigrid(10,10,flowOp.getA(),flowOp.getb(), params.getResidualTolerance(), dUdV, 20, 20, LinearSolver::vCycle);
LinearSolver::sparseMatSorNoResidual(flowOp.getA(), X0 ,dUdV, flowOp.getb(), params.getOverRelaxation(), params.getSorIters());
}
#if OPTFLOW_VERBOSE
std::cout<<" --- "<< (std::clock() - start) / (double)CLOCKS_PER_SEC <<'\n';
#endif
#if OPTFLOW_DEBUG
for(int i = 0; i < dUdV.size(); ++i){
if (!(dUdV.ptr[i] == dUdV.ptr[i])){
cout << "ERROR - NAN";
}
}
#endif
UtilsMat::clamp(dUdV, -1, 1);
memcpy(Du.data, dUdV.ptr, rows * cols * sizeof(float));
memcpy(Dv.data, dUdV.ptr + (rows * cols), rows * cols * sizeof(float));
flowUV UV0(UV);
UV.getU() += Du;
UV.getV() += Dv;
cv::Mat tmpU, tmpV;
UV.getU().copyTo(tmpU);
UV.getV().copyTo(tmpV);
WeightedMedianFilter::computeMedianFilter(UV.getU(), UV.getV(), Im1, Im2, params.getMedianFilterRadius());
Du = UV.getU() - UV0.getU();
Dv = UV.getV() - UV0.getV();
UV0.getU().copyTo(UV.getU());
UV0.getV().copyTo(UV.getV());
UV0.getU() += Du;
UV0.getV() += Dv;
if (params.isDisplay())
UtilsFlow::DrawFlow(UV0.getU(), UV0.getV(), "Flow");
}
UV.getU() += Du;
UV.getV() += Dv;
}
}