ROOT: Test code for sample Hough Transformation not around a point but a circle with given radius r

houghtest.cpp

#include <iostream>
#include <vector>
#include <cfloat>

void readPoints(string filename, std::vector<double> &x, std::vector<double> &y, std::vector<double> &r, int upToLineNumber = 2) {
	ifstream file(filename.c_str());
	float tempX, tempY, tempZ, tempR, tempI;
	char tempChar[10];
	int i = 1;
	while(i <= upToLineNumber && file >> tempX >> tempY >> tempZ >> tempR >> tempI >> tempChar) {
		x.push_back(tempX);
		y.push_back(tempY);
		r.push_back(tempR);
		i++;
	}
	file.close();
}

bool epsilon_equal_to(double value, double valueToCompareTo) {
	return (value >= (valueToCompareTo - DBL_EPSILON) && value <= (valueToCompareTo + DBL_EPSILON) );
}

double function_houghTrafo(double x, double y, double r, double alpha) {
	return sin(alpha * TMath::DegToRad()) * y + cos(alpha * TMath::DegToRad()) * x + r;
}

int houghtest() {
	std::vector<double> x;
	std::vector<double> y;
	std::vector<double> r;

	readPoints("data.dat", x, y, r, 18);

	std::cout << x.size() << std::endl;

	TGraph * g1 = new TGraph();

	for (int i = 0; i < x.size(); i++) {
		g1->SetPoint(i, x[i], y[i]);
	}

	g1->SetMarkerStyle(kFullDotMedium);
	g1->SetMarkerColor(kBlue);

	TF1 * preHough = new TF1("preHough", "-cos([0])/sin([0])*x+[1]/sin([0])");
	TF1 * tubedHough = new TF1("tubedHough", "sin(x)*[1]+cos(x)*[0]+[2]");

	TMultiGraph * mg = new TMultiGraph();

	std::vector<TGraph*> theGraphs;

	double everyXDegrees = 2;
	double upperDegree = 360;

	/**
	 * First off, some visualization
	 */
	for (std::vector<double>::iterator itX = x.begin(), itY = y.begin(), itR = r.begin(); itX != x.end() && itY != y.end() && itR != r.end(); itX++, itY++, itR++) { // this is really stupid, but I did like to test a more complex approach with iterators...
		tubedHough->SetParameters(*itX, *itY, *itR);
		preHough->SetRange(*itX-1, *itX+1);

		for (int i = 1; i < upperDegree/everyXDegrees; ++i) {
			double angle = i * everyXDegrees;
			if (epsilon_equal_to(angle, 180) || epsilon_equal_to(angle, 0) || epsilon_equal_to(angle, 360)) continue;
			std::cout << "angle = " << angle << std::endl;
			double angleDegToRad = angle * TMath::DegToRad();
			preHough->SetParameters(angleDegToRad, tubedHough->Eval(angleDegToRad));
			std::cout << "  parameters: " << "0 = " << preHough->GetParameters()[0] << ", 1 = " << preHough->GetParameters()[1] << " = " << tubedHough->Eval(angleDegToRad) << std::endl;
			TGraph * tempGraph = new TGraph(preHough);
			tempGraph->SetLineWidth(1);
			tempGraph->SetLineColor(kRed-10+(i%13));
			theGraphs.push_back(tempGraph);
		}
	}

	for (int i = 0; i < theGraphs.size(); ++i) {
		mg->Add(theGraphs[i]);
	}

	TCanvas * c1 = new TCanvas("c1","c1",0,0,500,500);
	mg->Add(g1,"P");
	mg->Draw("ALP");

	/**
	 * Let's start that hough transformation
	 */

	 std::vector<double> houghTransformedPoints;
	 std::vector<double> angles;

	 for (int i = 0; i < upperDegree/everyXDegrees; ++i)
	 {
	 	angles.push_back(i*everyXDegrees);
	 }

	 for (int i = 0; i < x.size(); ++i)
	 {
	 	for (int j = 0; j < upperDegree/everyXDegrees; ++j)
	 	{
	 		double singleHoughTransformedPoint = function_houghTrafo(x[i], y[i], r[i], angles[j]);
	 		houghTransformedPoints.push_back(singleHoughTransformedPoint);
	 	}
	 }

	 double ylow = *min_element(houghTransformedPoints.begin(),houghTransformedPoints.end());
	 double yup = *max_element(houghTransformedPoints.begin(), houghTransformedPoints.end());

	 TH2F * houghHist = new TH2F("houghHist", "Hough Hist", upperDegree/everyXDegrees, 0, upperDegree, upperDegree/everyXDegrees, ylow, yup);

	 for (int i = 0; i < houghTransformedPoints.size(); ++i)
	 {
	 	std::cout << "angle = " << angles[i%angles.size()] << ", pointValue = " << houghTransformedPoints[i] << std::endl;
	 	houghHist->Fill(angles[i%angles.size()], houghTransformedPoints[i]);
	 }

	 TCanvas * c2 = new TCanvas("c2","c2",505,0,500,500);

	 houghHist->Draw("COLZ");

	 int maxX, maxY, maxZ;
	 houghHist->GetMaximumBin(maxX, maxY, maxZ);
	 std::cout << maxX << " " << maxY << " " << maxZ << std::endl;
	 // houghHist->SetBinContent(maxX, maxY, 10);

	 double cellWidth = everyXDegrees/upperDegree;
	 double xlow = 0;
	 double xup = upperDegree;
	 double angle_mostPossible = xlow+(xup-xlow)*maxX*cellWidth;
	 double r_mostPossible = ylow+(yup-ylow)*maxY*cellWidth;
	 std::cout << "angle = " << angle_mostPossible << ", r = " << r_mostPossible << std::endl;
	 c1->cd(0);
	 // preHough->SetParameters(angle_mostPossible, r_mostPossible);
	 preHough->SetParameters(101*TMath::DegToRad(), -2.4); // read out by hand
	 preHough->SetRange(15, 40);
	 TGraph * probableLine = new TGraph(preHough);
	 probableLine->SetLineColor(kBlue);
	 probableLine->SetLineWidth(3);
	 probableLine->Draw("SAME");
	 preHough->Print();
	 probableLine->Print();

	return 1;
}