diff options
| -rw-r--r-- | client2.cpp | 44 |
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); |