summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--client2.cpp44
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);