cylindrical transformcylindrical

1 files changed, 39 insertions, 5 deletions

diff --git a/client2.cpp b/client2.cpp
index e412878..890e7fd 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -202,6 +202,7 @@ float fixup_angle(float a)
 
 void calc_undistort_maps(float px_per_deg, int width, int height, Mat& map1, Mat& map2)
 {
+	
 	Mat camera_matrix= Mat::zeros(3,3,CV_32FC1);
 	camera_matrix.at<float>(0,0)=1.0; //fx
 	camera_matrix.at<float>(1,1)=1.0; //fy
@@ -212,11 +213,38 @@ void calc_undistort_maps(float px_per_deg, int width, int height, Mat& map1, Mat
 	camera_matrix2.at<float>(0,2)=width/2.; //cx
 	camera_matrix2.at<float>(1,2)=height/2.; //cy
 
-	float px_per_rad = px_per_deg * PI / 180.;
+	float px_per_rad = px_per_deg * PI / 180.; // WTF?! no, *180/PI
 
 	Matx<float,1,5> dist_coeffs(-px_per_rad*px_per_rad/3.f, px_per_rad*px_per_rad*px_per_rad*px_per_rad/5.f, 0.f, 0.f, -px_per_rad*px_per_rad*px_per_rad*px_per_rad*px_per_rad*px_per_rad/7.f);
 
 	initUndistortRectifyMap(camera_matrix, dist_coeffs, Mat(), camera_matrix2, Size(width,height), CV_32FC1, map1, map2);
+	for (int i=0; i<50; i++)
+		for (int j=0; j<50; j++)
+		{
+			map1.at<float>(height-i,width-j)=100.0;
+			map2.at<float>(height-i,width-j)=100.0;
+		}
+}
+
+void calc_cyl_maps(float px_per_deg, int width, int height, Mat& map1, Mat& map2)
+{
+	//float px_per_rad = px_per_deg / PI * 180.;
+	float px_per_rad = 1280/(80./180.*PI);
+	float tan_maxalpha = tan( (width/2)/px_per_rad );
+
+	for (int u=0; u<width; u++)
+	{
+		for (int v=0; v<height; v++)
+		{
+			float alpha = (u-width/2)/px_per_rad;
+			float x = tan(alpha) / tan_maxalpha * width/2;
+			float y = cos(alpha) * (v-height/2);
+			map1.at<float>(v,u) = x+width/2;
+			map2.at<float>(v,u) = y+height/2;
+		}
+		printf("%i ", u); fflush(stdout);
+	}
+	printf("\n");
 }
 
 int main(int argc, const char** argv)
@@ -295,7 +323,9 @@ int main(int argc, const char** argv)
 
 	Mat white(Size(1280,720), CV_8UC3, Scalar(255,255,255));
 	Mat map1(Size(width,height), CV_32FC1), map2(Size(width,height), CV_32FC1);
+	Mat map3(Size(width,height), CV_32FC1), map4(Size(width,height), CV_32FC1);
 	calc_undistort_maps(80/1280., 1280,720, map1, map2);
+	calc_cyl_maps(80, 1280,720, map3, map4);
 
 	float scale_factor = 0.2;
 	float diag = sqrt(1280*1280+720*720);
@@ -312,7 +342,7 @@ int main(int argc, const char** argv)
 	int total_x = 100, total_y = 00;
 	float total_rot = 0.0;
 
-	Mat frame(Size(1280,720), CV_8UC3), frame_(Size(1280,720), CV_8UC3), gray, oldgray;
+	Mat frame(Size(1280,720), CV_8UC3), frame_(Size(1280,720), CV_8UC3), frame__(Size(1280,720), CV_8UC3), gray, oldgray;
 	Mat screencontent(real_canvas_height,real_canvas_width, CV_8UC3);
 	Mat screencontent_(real_canvas_height,real_canvas_width, CV_8UC3);
 	Mat screencontent_mask(real_canvas_height,real_canvas_width, CV_8UC3);
@@ -335,7 +365,8 @@ int main(int argc, const char** argv)
 	for (int i=0; i<400;i++)
 	{
 		drone.get(frame_, &navdata);
-		remap(frame_, frame, map1, map2, INTER_LINEAR);
+		remap(frame_, frame__, map1, map2, INTER_LINEAR);
+		remap(frame__, frame, map3, map4, INTER_LINEAR);
 		cvtColor(frame, oldgray, COLOR_BGR2GRAY);
 	}
 
@@ -359,7 +390,10 @@ int main(int argc, const char** argv)
 		//for (int i=0; i<1280; i+=50) frame_.col(i)=Scalar(0,255,255);
 		//for (int i=0; i<720; i+=50) frame_.row(i)=Scalar(0,255,255);
 
-		remap(frame_, frame, map1, map2, INTER_LINEAR);
+		remap(frame_, frame__, map1, map2, INTER_LINEAR);
+		for (int i=0; i<1280; i+=50) frame__.col(i)=Scalar(0,255,255);
+		for (int i=0; i<720; i+=50) frame__.row(i)=Scalar(0,255,255);
+		remap(frame__, frame, map3, map4, INTER_LINEAR);
 		cvtColor(frame, gray, COLOR_BGR2GRAY);
 
 
@@ -466,7 +500,7 @@ int main(int argc, const char** argv)
 		if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS)
 			glfwSetWindowShouldClose(window, GL_TRUE);
 
-		imshow("dingens",frame_);
+		imshow("dingens",frame);