// Variance Ratio
double getVR(Mat hist1,Mat hist2)
{
Mat idx=Mat::zeros(hist1.rows,hist1.cols,CV_32FC1);
for (int i=0;i<idx.rows;++i)
{
float* r_ptr=idx.ptr<float>(i);
r_ptr[0]=(float)i;
}
double mean_idx=hist1.dot(idx);
Mat temp=idx-mean_idx;
temp=temp.mul(temp);
double variance1=hist1.dot(temp);
mean_idx=hist2.dot(idx);
temp=idx-mean_idx;
temp=temp.mul(temp);
double variance2=hist2.dot(temp);
Mat hist_mean=(hist1+hist2)*0.5;
mean_idx=hist_mean.dot(idx);
temp=idx-mean_idx;
temp=temp.mul(temp);
double variance_mean=hist_mean.dot(temp);
return variance_mean/(variance1+variance2);
}
Mat UpdateCompetition(Mat Transformed_Templates, Mat BackwardTransform, Mat g, int r, double k, Mat TranSc,double p){
int count = Transformed_Templates.rows;
g.convertTo(g,CV_32F);
Mat subtracted_g(g.rows, g.cols, CV_64FC1);
Mat thresholded_g(g.rows, g.cols, CV_32FC1);
double Thresh_VAL = 0.1;
double MAX_VAL = 1;
Mat q(g.rows, g.cols, CV_32F);
double T_L2;
double BackwardTransform_L2;
double min, max;
if (dispsc)
cout << TranSc << endl;
for(int i=0; i<count; i++){
if (g.at<float>(0, i) == 0) {
q.at<float>(0, i) = 0;
continue;
}
Mat T = Transformed_Templates.row(i).reshape(0,r);
T.convertTo(T,CV_32FC1);
//T_L2 = norm(T, NORM_L2);
T_L2 = sum(T)[0];
//T_L2 = sum(T)[0];
//BackwardTransform_L2 = norm(BackwardTransform, NORM_L2);
BackwardTransform_L2 = sum(BackwardTransform)[0];
if (test_comp) {
imshow("T", T);
imshow("BackwardTransform", BackwardTransform);
waitKey();
}
//cout << TranSc << endl;
if(BackwardTransform_L2 !=0 && T_L2 != 0){
if (startscale)
q.at<float>(0,i) = T.dot(BackwardTransform)*((TranSc.at<float>(0,i)));
//q.at<double>(0, i) = T.dot(BackwardTransform) / T_L2;
else
q.at<float>(0, i) = T.dot(BackwardTransform);
}else{
q.at<float>(0,i) = 0;
}
if (startscale)
if (TranSc.at<float>(0,i) < 1)
p = 1.5;
}
//cout<<"q: "<<q<<endl;
minMaxLoc(q, &min, &max);
// cout<<"q_min:"<<min<<" q_max: "<<max<<endl;
Mat temp;
pow(1- q / max, p, temp);
subtract(g, k*(temp), subtracted_g) ;
//cout << "g:" << g << " subtracted_g: " << subtracted_g << endl;
subtracted_g.convertTo(subtracted_g,CV_32F);
threshold(subtracted_g, thresholded_g, Thresh_VAL, MAX_VAL, THRESH_TOZERO);
return thresholded_g;
}
void MarkerLocator::ChangeSpace(Mat& r_point)
{
double wz = r_point.dot(m_robotForward) / m_robotForward.dot(m_robotForward);
double wx = r_point.dot(m_robotSide) / m_robotSide.dot(m_robotSide);
r_point = (Mat_<double>(3, 1) << wx, 0.0, wz);
}
Mat cal_3d_point2line_intersect(Mat l_first_point, Mat l_second_point, Mat point) {
Mat v = l_second_point - l_first_point;
Mat w = point - l_first_point;
double c1 = w.dot(v);
double c2 = v.dot(v);
double b = c1 / c2;
return l_first_point + b * v;
}
float cal_3d_point2line_distance(Mat l_first_point, Mat l_second_point, Mat point) {
Mat v = l_second_point - l_first_point;
Mat w = point - l_first_point;
double c1 = w.dot(v);
double c2 = v.dot(v);
double b = c1 / c2;
Mat norm = point - (l_first_point + b * v);
return norm.dot(norm);
}
float CalcDistanceToFeatureLine(Mat f1, Mat f2, Mat fx)
{
Mat a = fx - f1;
Mat b = f2 - f1;
double numerator = a.dot(b);
double denominator = b.dot(b);
float muy = (float)(numerator / denominator);
Mat px = f1 + muy*(f2 - f1);
return CalcVectorMagnitude((fx - px));
}
double calculateArea(Mat img,int col,int row)
{
Mat A = Mat::ones(row, col, CV_8U);
return A.dot(img);
}
float SVMSGDImpl::calcShift(InputArray _samples, InputArray _responses) const
{
float margin[2] = { std::numeric_limits<float>::max(), std::numeric_limits<float>::max() };
Mat trainSamples = _samples.getMat();
int trainSamplesCount = trainSamples.rows;
Mat trainResponses = _responses.getMat();
CV_Assert(trainResponses.type() == CV_32FC1);
for (int samplesIndex = 0; samplesIndex < trainSamplesCount; samplesIndex++)
{
Mat currentSample = trainSamples.row(samplesIndex);
float dotProduct = static_cast<float>(currentSample.dot(weights_));
bool positive = isPositive(trainResponses.at<float>(samplesIndex));
int index = positive ? 0 : 1;
float signToMul = positive ? 1.f : -1.f;
float curMargin = dotProduct * signToMul;
if (curMargin < margin[index])
{
margin[index] = curMargin;
}
}
return -(margin[0] - margin[1]) / 2.f;
}
float SVMSGDImpl::predict( InputArray _samples, OutputArray _results, int ) const
{
float result = 0;
cv::Mat samples = _samples.getMat();
int nSamples = samples.rows;
cv::Mat results;
CV_Assert( samples.cols == weights_.cols && samples.type() == CV_32FC1);
if( _results.needed() )
{
_results.create( nSamples, 1, samples.type() );
results = _results.getMat();
}
else
{
CV_Assert( nSamples == 1 );
results = Mat(1, 1, CV_32FC1, &result);
}
for (int sampleIndex = 0; sampleIndex < nSamples; sampleIndex++)
{
Mat currentSample = samples.row(sampleIndex);
float criterion = static_cast<float>(currentSample.dot(weights_)) + shift_;
results.at<float>(sampleIndex) = (criterion >= 0) ? 1.f : -1.f;
}
return result;
}
/* Convolutions */
Mat Image::convolution1D1C(Mat &input, Mat &mask, bool reflected)
{
// Expand the matrix
Mat expanded, copy_input;
int borderType = BORDER_CONSTANT;
int offset = (mask.cols - 1) / 2;
if (reflected)
borderType = BORDER_REFLECT;
copyMakeBorder(input,expanded,0,0,offset,offset,borderType,0);
// Convolution!
Mat ROI;
Mat output = Mat::zeros(1, input.cols, CV_32FC1);
expanded.convertTo(expanded,CV_32FC1);
for (int i = 0; i < input.cols; i++) // Index are OK
{
ROI = Mat(expanded, Rect(i,0,mask.cols,1));
output.at<float>(Point(i,0)) = ROI.dot(mask);
}
return output;
}
void apply5Filter(Mat source, Mat filter, Mat result){
//dims of source mat
int h = source.rows;
int w = source.cols;
Mat temp;
double dot;
//allocate space for result mat
//incase result and source are same vector, we don't overwrite result
Mat tempResult(h, w, CV_64F);
//add border to source image for filtering
copyMakeBorder(source, temp, 2, 2, 2, 2, BORDER_REPLICATE);
//filter source mat and store in result mat pixel by pixel
for(int i = 0; i < h; i++){
for(int j = 0; j < w; j++){
dot = filter.dot(temp.rowRange(i, i+5).colRange(j, j+5));
tempResult.at<double>(i, j) = dot;
}
}
//overwrite result
tempResult.copyTo(result);
}
//==============================================================================
void
patch_model::
train(const vector<Mat> &images,
const Size psize,
const float var,
const float lambda,
const float mu_init,
const int nsamples,
const bool visi)
{
int N = images.size(),n = psize.width*psize.height;
//compute desired response map
Size wsize = images[0].size();
if((wsize.width < psize.width) || (wsize.height < psize.height)){
cerr << "Invalid image size < patch size!" << endl; throw std::exception();
}
int dx = wsize.width-psize.width,dy = wsize.height-psize.height;
Mat F(dy,dx,CV_32F);
for(int y = 0; y < dy; y++){ float vy = (dy-1)/2 - y;
for(int x = 0; x < dx; x++){ float vx = (dx-1)/2 - x;
F.fl(y,x) = exp(-0.5*(vx*vx+vy*vy)/var);
}
}
normalize(F,F,0,1,NORM_MINMAX);
//allocate memory
Mat I(wsize.height,wsize.width,CV_32F);
Mat dP(psize.height,psize.width,CV_32F);
Mat O = Mat::ones(psize.height,psize.width,CV_32F)/n;
P = Mat::zeros(psize.height,psize.width,CV_32F);
//optimise using stochastic gradient descent
RNG rn(getTickCount()); double mu=mu_init,step=pow(1e-8/mu_init,1.0/nsamples);
for(int sample = 0; sample < nsamples; sample++){ int i = rn.uniform(0,N);
I = this->convert_image(images[i]); dP = 0.0;
for(int y = 0; y < dy; y++){
for(int x = 0; x < dx; x++){
Mat Wi = I(Rect(x,y,psize.width,psize.height)).clone();
Wi -= Wi.dot(O); normalize(Wi,Wi);
dP += (F.fl(y,x) - P.dot(Wi))*Wi;
}
}
P += mu*(dP - lambda*P); mu *= step;
if(visi){
Mat R; matchTemplate(I,P,R,CV_TM_CCOEFF_NORMED);
Mat PP; normalize(P,PP,0,1,NORM_MINMAX);
normalize(dP,dP,0,1,NORM_MINMAX);
normalize(R,R,0,1,NORM_MINMAX);
imshow("P",PP); imshow("dP",dP); imshow("R",R);
if(waitKey(10) == 27)break;
}
}return;
}
static void project_onto_jacobian_ECC(const Mat& src1, const Mat& src2, Mat& dst)
{
/* this functions is used for two types of projections. If src1.cols ==src.cols
it does a blockwise multiplication (like in the outer product of vectors)
of the blocks in matrices src1 and src2 and dst
has size (number_of_blcks x number_of_blocks), otherwise dst is a vector of size
(number_of_blocks x 1) since src2 is "multiplied"(dot) with each block of src1.
The number_of_blocks is equal to the number of parameters we are lloking for
(i.e. rtanslation:2, euclidean: 3, affine: 6, homography: 8)
*/
CV_Assert(src1.rows == src2.rows);
CV_Assert((src1.cols % src2.cols) == 0);
int w;
float* dstPtr = dst.ptr<float>(0);
if (src1.cols !=src2.cols){//dst.cols==1
w = src2.cols;
for (int i=0; i<dst.rows; i++){
dstPtr[i] = (float) src2.dot(src1.colRange(i*w,(i+1)*w));
}
}
else {
CV_Assert(dst.cols == dst.rows); //dst is square (and symmetric)
w = src2.cols/dst.cols;
Mat mat;
for (int i=0; i<dst.rows; i++){
mat = Mat(src1.colRange(i*w, (i+1)*w));
dstPtr[i*(dst.rows+1)] = (float) pow(norm(mat),2); //diagonal elements
for (int j=i+1; j<dst.cols; j++){ //j starts from i+1
dstPtr[i*dst.cols+j] = (float) mat.dot(src2.colRange(j*w, (j+1)*w));
dstPtr[j*dst.cols+i] = dstPtr[i*dst.cols+j]; //due to symmetry
}
}
}
}
/*
void point3d2Mat(const Point3d& src, Mat& dest)
{
dest.create(3,1,CV_64F);
dest.at<double>(0,0)=src.x;
dest.at<double>(1,0)=src.y;
dest.at<double>(2,0)=src.z;
}
void setXYZ(Mat& in, double&x, double&y, double&z)
{
x=in.at<double>(0,0);
y=in.at<double>(1,0);
z=in.at<double>(2,0);
// cout<<format("set XYZ: %.04f %.04f %.04f\n",x,y,z);
}
void lookatBF(const Point3d& from, const Point3d& to, Mat& destR)
{
double x,y,z;
Mat fromMat;
Mat toMat;
point3d2Mat(from,fromMat);
point3d2Mat(to,toMat);
Mat fromtoMat;
add(toMat,fromMat,fromtoMat,Mat(),CV_64F);
double ndiv = 1.0/norm(fromtoMat);
fromtoMat*=ndiv;
setXYZ(fromtoMat,x,y,z);
destR = Mat::eye(3,3,CV_64F);
double yaw =-z/abs(z)*asin(y/sqrt(y*y+z*z))/CV_PI*180.0;
rotYaw(destR,destR,yaw);
Mat RfromtoMat = destR*fromtoMat;
setXYZ(RfromtoMat,x,y,z);
double pitch =z/abs(z)*asin(x/sqrt(x*x+z*z))/CV_PI*180.0;
rotPitch(destR,destR,pitch);
}
*/
void lookat(const Point3d& from, const Point3d& to, Mat& destR)
{
Mat destMat = Mat(Point3d(0.0, 0.0, 1.0));
Mat srcMat = Mat(from + to);
srcMat = srcMat / norm(srcMat);
Mat rotaxis = srcMat.cross(destMat);
double angle = acos(srcMat.dot(destMat));
//normalize cross product and multiply rotation angle
rotaxis = rotaxis / norm(rotaxis)*angle;
Rodrigues(rotaxis, destR);
}
void MarkerLocator::TVectToRangeBaring(Mat r_tVec, double& rr_range, double& rr_bearing)
{
// Get the vector relative to the robot rather than the camera
subtract(r_tVec, m_robotLoc, r_tVec);
// Start by projecting our transform vector onto the robot's plane
ChangeSpace(r_tVec);
// Range is easy
rr_range = sqrt(r_tVec.dot(r_tVec));
rr_bearing = atan(r_tVec.at<double>(0, 0)/r_tVec.at<double>(2, 0));
}
/*
* Correlación del vector signal usando la máscara mask
*/
void crossCorrelation1D(Mat &signal, Mat &mask) {
int offset_mask = mask.cols / 2;
Mat signal_copy;
signal.copyTo(signal_copy);
for (int i = offset_mask - 1; i < signal.cols - offset_mask - 1; i++) {
Mat roi = Mat(signal_copy, Rect(i - offset_mask + 1, 0, mask.cols, 1));
double dot_product = mask.dot(roi); //Producto escalar entre la máscara y la sección de la imagen
signal.at<float>(Point(i, 0)) = dot_product;
}
}
/*
src: input image
kernel: filter kernel
return: convolution result
*/
Mat Dip3::spatialConvolution(Mat& src, Mat& kernel){
auto forEachMat = [](Mat img, int xstep, int ystep, function<Mat (Mat orig, Mat copy, int x, int y)> func) -> Mat {
Mat copy = img.clone();
for (int x = 0; x < img.cols; x+=xstep) {
for (int y = 0; y < img.rows; y+=ystep) {
func(img, copy, x, y);
}
}
return copy;
};
auto convolution = [kernel](Mat orig, Mat copy, int x, int y) -> Mat {
Mat defaultMat = Mat::ones(kernel.rows, kernel.cols, CV_32FC1);
int center = kernel.rows/2;
// border handling using default values
for (int i = 0; i < kernel.rows; i++) for (int j = 0; j < kernel.cols; j++) {
if ((x + i) >= center && (x + i) < (orig.rows + center) &&
(y + j) >= center && (y + j) < (orig.cols + center)) {
defaultMat.at<float>(i, j) = orig.at<float>(x + i - center, y + j - center);
}
}
Mat flippedKernel;
flip(kernel, flippedKernel, -1);
float result = sum(defaultMat.dot(flippedKernel)).val[0];
copy.at<float>(x, y) = result;
return copy;
};
Mat copy = src.clone();
Mat result = forEachMat(copy, 1, 1, convolution);
return result;
}
/*
* Correlación del vector signal usando la máscara mask
*/
void crossCorrelation(Mat &input, Mat &mask) {
int offset_mask = mask.cols / 2;
Mat input_copy;
input.copyTo(input_copy);
for (int i = offset_mask - 1; i < input.rows - offset_mask - 1; i++) {
for (int j = offset_mask - 1; j < input.cols - offset_mask - 1; j++) {
Mat roi = Mat(input_copy, Rect(j - offset_mask + 1, i - offset_mask + 1, mask.cols, mask.rows));
double dot_product = mask.dot(roi); //Producto escalar entre la máscara y la sección de la imagen
input.at<float>(Point(j,i)) = dot_product;
}
}
}
void SVMSGDImpl::updateWeights(InputArray _sample, bool positive, float stepSize, Mat& weights)
{
Mat sample = _sample.getMat();
int response = positive ? 1 : -1; // ensure that trainResponses are -1 or 1
if ( sample.dot(weights) * response > 1)
{
// Not a support vector, only apply weight decay
weights *= (1.f - stepSize * params.marginRegularization);
}
else
{
// It's a support vector, add it to the weights
weights -= (stepSize * params.marginRegularization) * weights - (stepSize * response) * sample;
}
}
double get_point_line_distance(cv::Vec4f line, cv::Point point)
{
using namespace cv;
Point2f linePoint(line[2], line[3]);
Mat lineDirection(Point2f(line[0], line[1]));
Mat normedLineDirection;
normalize(lineDirection, normedLineDirection);
;
Mat pointDifference(linePoint - (Point2f) point);
pointDifference.at<float>(0, 0);
double differenceLength = pow(pow(pointDifference.at<float>(0, 0), 2) + pow(pointDifference.at<float>(1, 0), 2), 0.5);
Mat normedDifference;
normalize(pointDifference, normedDifference);
//std::cout << "Line " << normedLineDirection << std::endl;
//std::cout << "normed" << normedDifference << std::endl;
double dotProduct = normedLineDirection.dot(normedDifference);
assert(abs(dotProduct) <= 1.001);
if (dotProduct > 1)
{
dotProduct = 1;
}
else if (dotProduct < -1)
{
dotProduct = -1;
}
//std::cout << "Dot product: " << dotProduct << std::endl;
double angle = acos(dotProduct);
//std::cout << "Angle: " << angle << std::endl;
double distance = differenceLength * sin(angle);
//std::cout << "Adding distance " << distance << std::endl;
return distance;
}
/*
* Método para contar el número de puntos con coordenadas negativas para un vector t y matriz R
*/
int countNegativeDepth(vector<Point> &points_1_2,vector<Point> &points_2_1,Mat &t,Mat &R, float focal_distance_1, float focal_distance_2){
int counter = 0;
for(int i = 0; i < points_1_2.size(); i++){
Mat homogeneous = Mat(1,3, CV_64F);
homogeneous.at<double>(0) = points_1_2.at(i).x;
homogeneous.at<double>(1) = points_1_2.at(i).y;
homogeneous.at<double>(2) = 1;
double depth_i = focal_distance_2 * ( focal_distance_1 * t.dot(R.row(0) - points_2_1.at(i).x * R.row(2)) ) / ( focal_distance_2 * homogeneous.dot( R.row(0) - points_2_1.at(i).x * R.row(2)));
double depth_d = R.row(2).dot(focal_distance_1/depth_i * homogeneous -t);
if( depth_i < 0 or depth_d < 0){
counter++;
}
}
return counter;
}
void fconv_1( const Mat &A, const Mat &F, Mat &R )
{
int RowA = A.rows, ColA = A.cols, NumFeatures = A.channels();
int RowF = F.rows, ColF = F.cols, ChnF = F.channels();
if( NumFeatures!=ChnF )
throw runtime_error("");
int RowR = RowA - RowF + 1, ColR = ColA - ColF + 1;
float *Rpt = (float*)R.data;
int Rstep = R.step1();
for( int r=0; r!=RowR; r++ ){
float *pt = Rpt + r*Rstep;
for( int c=0; c!=ColR; c++ ){
Mat Asub = A( Rect(c,r,ColF,RowF) );
*(pt++) = (float)( F.dot( Asub ) );
}
}
}
vector< Point_<T> > orderCornersImpl(const vector< Point_<T> >& corners){
vector< Point_<T> > orderedCorners;
Mat center = Mat::zeros( 1,3,CV_64F );
for(size_t i = 0; i < corners.size(); i++){
center.at<double>(0,0) += corners[i].x;
center.at<double>(0,1) += corners[i].y;
}
center /= corners.size();
Mat p0 = (Mat_<double>(1,3) << corners[0].x, corners[0].y, 0 );
Mat p1 = (Mat_<double>(1,3) << corners[1].x, corners[1].y, 0 );
if((center - p0).cross(p1 - p0).at<double>(0,2) < 0){ //Double-check this math just in case
orderedCorners = vector< Point_<T> >(corners.begin(), corners.end());
}
else{
orderedCorners = vector< Point_<T> >(corners.rbegin(), corners.rend());
}
int shift = 0;
double tlMax = 0;
Mat B = (Mat_<double>(1,2) << -1, -1);
for(size_t i = 0; i < orderedCorners.size(); i++ ){
Mat A = (Mat_<double>(1,2) << orderedCorners[i].x - center.at<double>(0,0), orderedCorners[i].y - center.at<double>(0,1));
double tlProj = A.dot(B);
if(tlProj > tlMax){
shift = i;
tlMax = tlProj;
}
}
vector< Point_<T> > temp = vector< Point_<T> >(orderedCorners.begin(), orderedCorners.end());
for(size_t i = 0; i < orderedCorners.size(); i++ ){
orderedCorners[i] = temp[(i + shift) % orderedCorners.size()];
}
return orderedCorners;
}
Tracker::Topographic Tracker::TopographicClassification( Mat grad, double eval1, double eval2, Mat evec1, Mat evec2 )
{
double mag = norm(grad);
if (mag < t_mag_ && eval1 < -1*t_ev_ && eval2 < -1*t_ev_)
return PEAK;
else if (mag < t_mag_ && eval1 > t_ev_ && eval2 > t_ev_)
return PIT;
else if (mag < t_mag_ && eval1 * eval2 < 0) {
if (eval1 + eval2 < 0)
return RIDGESADDLE;
else return RAVINESADDLE;
}
else if ((mag >= t_mag_ && eval1 < -1 * t_ev_ && fabs(grad.dot(evec1)) < t_ge_) ||
(mag >= t_mag_ && eval2 < -1 * t_ev_ && fabs(grad.dot(evec2)) < t_ge_) ||
(mag < t_mag_ && eval1 < -1 * t_ev_ && fabs(eval2) <= t_ev_) ||
(mag < t_mag_ && fabs(eval1) <= t_ev_ && eval2 < -1 * t_ev_))
return RIDGE;
else if ((mag >= t_mag_ && eval1 > t_ev_ && fabs(grad.dot(evec1)) < t_ge_) ||
(mag >= t_mag_ && eval2 > t_ev_ && fabs(grad.dot(evec2)) < t_ge_) ||
(mag < t_mag_ && eval1 > t_ev_ && fabs(eval2) <= t_ev_) ||
(mag < t_mag_ && fabs(eval1) <= t_ev_ && eval2 > t_ev_))
return RAVINE;
else if (mag < t_mag_ && fabs(eval1) <= t_ev_ && fabs(eval2) <= t_ev_)
return FLAT;
else if ((fabs(grad.dot(evec1)) >= t_ge_ && fabs(grad.dot(evec2)) >= t_ge_) ||
(fabs(grad.dot(evec1)) >= t_ge_ && fabs(eval2) <= t_ev_) ||
(fabs(grad.dot(evec2)) >= t_ge_ && fabs(eval1) <= t_ev_) ||
(mag >= t_mag_ && fabs(eval1) <= t_ev_ && fabs(eval2) <= t_ev_)) {
if (fabs(eval1) <= t_ev_ && fabs(eval2) <= t_ev_)
return SLOPEHILL;
else if ((eval1 > 0 && eval2 >= 0) || (eval1 >= 0 && eval2 > 0))
return CONVEXHILL;
else if ((eval1 < 0 && eval2 <= 0) || (eval1 <= 0 && eval2 < 0))
return CONCAVEHILL;
else if (eval1 * eval2 < 0) {
if (eval1 + eval2 < 0)
return CONVEXSADDLEHILL;
else return CONCAVESADDLEHILL;
}
}
else return UNKNOWN;
}
//ncc cost function
//返回(row,col)处像素点，视差为d时的cost value
double ncc(int row,int col,int d)
{
int col1 = col;
int row1 = row;
int col2 = col1 - d;
int row2 = row1;
if (col1 - r >= 0 && col1 + r < width && col2 - r >= 0 && col2 + r < width
&& row1 - r >= 0 && row1 + r < height && row2 - r >= 0 && row2 + r < height)
{
Mat cl;
i1(Rect(col1-r,row1-r,2*r+1,2*r+1)).copyTo(cl);
cl=cl.reshape(1,1);
Mat cr;
i2(Rect(col2-r,row2-r,2*r+1,2*r+1)).copyTo(cr);
cr=cr.reshape(1,1);
cl=cl-mean(cl);
cr=cr-mean(cr);
return 1-cl.dot(cr)/(norm(cl)*norm(cr));
}else
{
return 2;
}
}
Mat *Mat::pinv()
{
Mat *Ans = new Mat(wid, len);
Mat *TransJ = new Mat(wid, len);
TransJ->Copy2(trans());
Mat *temp = new Mat(wid, wid);
temp->Copy2(dot(TransJ));
temp->Copy2(temp->inv());
Ans->Copy2(TransJ->dot(temp));
delete TransJ;
delete temp;
return Ans;
}
Mat UpdateCompetition_Memory(Mat Transformed_Templates, Mat BackwardTransform, Mat g, int r, double k){
int count = Transformed_Templates.rows;
g.convertTo(g,CV_64F);
Mat subtracted_g(g.rows, g.cols, CV_64FC1);
Mat thresholded_g(g.rows, g.cols, CV_32FC1);
double Thresh_VAL = 0.3;
Mat q(g.rows, g.cols, CV_64F);
double min, max;
double T_L2;
double BackwardTransform_L2;
//printf("About to call dot in MSC\n");
//cout<<"g_memory: "<<g<<endl;
for(int i=0; i<count; i++){
Mat T = g.at<double>(0,i)*Transformed_Templates.row(i).reshape(0,r);
T.convertTo(T,CV_32FC1);
BackwardTransform.convertTo(BackwardTransform, CV_32FC1);
//cvWaitKey(0);
T_L2 = norm(T, NORM_L2);
BackwardTransform_L2 = norm(BackwardTransform, NORM_L2);
if(BackwardTransform_L2 !=0 && T_L2 != 0){
q.at<double>(0,i) = T.dot(BackwardTransform)/(T_L2*BackwardTransform_L2);
}else{
q.at<double>(0,i) = 0;
}
//printf("Dot product done for iteration: %d\n", i);
}
//printf("Dot product has been completed\n");
minMaxLoc(q, &min, &max);
//cout<<"in update MEMORY q_min:"<<min<<" q_max: "<<max<<endl;
subtract(g, k*(1-q/max), subtracted_g);
//cout<<"q: "<<q<<endl;
subtracted_g.convertTo(subtracted_g,CV_32FC1);
threshold(subtracted_g, thresholded_g, Thresh_VAL, MAX_VAL, THRESH_TOZERO);
return thresholded_g;
}
int main(){
// Matrix initialization
float a[] = {3,2,5,6,5,8,2,3,4};
Mat A = Mat(3, 3, CV_32FC1, a);
cout << "A: " << endl << A << endl << endl;
float b[] = {7,4,3,5,3,2,1,2,9};
Mat B = Mat(3, 3, CV_32FC1, b);
cout << "B: " << endl << B << endl << endl;
Mat C;
// Vector initialization
float d[] = {2,4,1};
Mat D = Mat(3, 1, CV_32FC1, d);
cout << "D: " << endl << D << endl << endl;
float e[] = {5,2,9};
Mat E = Mat(3, 1, CV_32FC1, e);
cout << "E: " << endl << E << endl << endl;
Mat F;
// Matrix and vector multiplication
cout << "Matrix-vector multiplication: " << A*D << endl << endl;
// Matrix zeros
C = Mat::zeros(3,3,CV_32FC1);
cout << "Matrix Zeros: " << endl << C << endl << endl;
// Matrix ones
C = Mat::ones(3,3,CV_32FC1);
cout << "Matrix Ones: " << endl << C << endl << endl;
// Matrix identity
C = Mat::eye(3,3,CV_32FC1);
cout << "Matrix Identity: " << endl << C << endl << endl;
// Matrix addition
C = A + B;
cout << "Addition: " << endl << C << endl << endl;
// Matrix multiplication
C = A * B;
cout << "Multiplication: " << endl << C << endl << endl;
// Matrix multiplication per element
C = A.mul(B);
cout << "Multiplication 1 to 1: " << endl << C << endl << endl;
// Cross product
F = D.cross(E);
cout << "Cross product: " << endl << F << endl << endl;
// Dot product
F = D.dot(E);
cout << "Dot product: " << endl << F << endl << endl;
// Matrix inverse
C = A.inv();
cout << "Inverse: " << endl << C << endl << endl;
// Matrix transpose
C = A.t();
cout << "Transpose: " << endl << C << endl << endl;
// Matrix determinant
cout << "Determinant: " << endl << determinant(A) << endl << endl;
// Vector normalization
normalize(D, F);
cout << "Normalization: " << endl << F << endl << endl;
return 0;
}
int SL_MSC(Mat Input_Image, Mat Memory_Images, Size img_size, Mat *Fwd_Path, Mat *Bwd_Path, TransformationSet & finalTrans){
t_total = clock();
t_loop = 0;
Mat affine_transformation;
double MAX_VAL = 255;
Mat transformations;
vector < Mat > transformation_set;
Mat G_layer; // Competition function values for each layer.
vector< Mat > G; // The competition function
int iteration_count = 100; // Number of iterations for which MSC will operate.
int ret = -1;
double verified_ret;
FILE *fp;
double dot_product_input_object = Input_Image.dot(Input_Image);
int layer_count = 1+(int)(xTranslate_layer)+(int)(yTranslate_layer)+(int)(rotate_layer)+(int)(scale_layer);
//transformations.release();
//G_layer.release();
k_transformations = new double[layer_count-1];
// k_transformations[layer_count-1] = k_memory;
getTransform(img_size, transformation_set, G, finalTrans);
//transformation_set.clear();
vector<Mat>().swap(transformation_set);
/* get the mapping for transformation*/
vector<Mat> MapForw, MapBack;
MapForw.reserve(layer_count - 1);
MapBack.reserve(layer_count - 1);
getTransMap(img_size, FORWARD, MapForw);
getTransMap(img_size, BACKWARD, MapBack);
int* idxTrans = new int[layer_count - 1];
FPV = new Fwd_Path_Values[layer_count];
FPV[0].Fwd_Superposition = Input_Image.clone();
for (int i = 1; i < layer_count; i++) {
FPV[i].Fwd_Superposition = Mat::zeros(Input_Image.rows, Input_Image.cols, CV_32F);
FPV[i].Transformed_Templates = Mat::zeros(Size(Input_Image.rows*Input_Image.cols, G[i-1].cols), CV_32F);
}
int count = 0;
while(iteration_count > 0){
iteration_count--;
//printf("About to call MSC %d\n",count++);
ret = MapSeekingCircuit(Input_Image, Memory_Images, img_size, Fwd_Path, Bwd_Path, layer_count, MapForw,MapBack, &G, k_transformations);
bool flag = 1; /* 1 for stopping the msc*/
if(iteration_count %5 == 0){
///* only inspect before the scaling layer*/
for (int kk = 0; kk < G.size(); kk++) {
//cout << "-------\n" << G[kk] << "-------\n";
if (countNonZero(G[kk]) != 1) {
flag = 0;
break;
}
else {
vector<Point> idx;
Mat current;
G[kk].convertTo(current, CV_8UC1, 100);
//cout << current << endl << G[kk] << endl;
findNonZero(current, idx);
idxTrans[kk] = (idx[0]).x;
}
}
if (dispInMid) {
imshow("FPV_Forward[1]", (*Fwd_Path) * 255);
imshow("BPV[1]", (*Bwd_Path) * 255);
cvWaitKey(0);
}
/* stop iteration condition: only one transformation is left*/
/* record the final transformation*/
if (flag) {
double xT = -xT_val[idxTrans[0]];
double yT = -yT_val[idxTrans[1]];
double ang = -rot_val[idxTrans[2]];
double sc;
if (scale_layer)
sc = sc_val[idxTrans[3]];
else
sc = 1;
//double xT = 0;
//double yT = 0;
//double ang = 0;
//double sc = 1;
finalTrans = TransformationSet(xT, yT, ang, sc);
break;
}
}
//printf("MSC dot products are done\n");
verified_ret = Verify_Object(Input_Image, *Bwd_Path, dot_product_input_object);
/*
if(verified_ret == 0){
printf("Image not recognized\n");
}else{
printf("Everything seems to be fine\n");
}
*/
}
ret = MapSeekingCircuit(Input_Image, Memory_Images, img_size, Fwd_Path, Bwd_Path, layer_count, MapForw, MapBack, &G, k_transformations);
//printf("The value of verified_ret is %g\n", verified_ret);
//xT_val.clear();
//yT_val.clear();
//rot_val.clear();
//sc_val.clear();
vector<double>().swap(xT_val);
vector<double>().swap(yT_val);
vector<double>().swap(rot_val);
vector<double>().swap(sc_val);
//MapForw.clear();
vector<Mat>().swap(MapForw);
//MapBack.clear();
//G.clear();
vector<Mat>().swap(MapBack);
vector<Mat>().swap(G);
t_total = clock()-t_total;
printf("it takes %d/%d for loop\n", t_loop, t_total);
return ret;
}
static inline
double normL2sq(const Mat &r)
{
return r.dot(r);
}
