summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--client2.cpp28
1 files changed, 24 insertions, 4 deletions
diff --git a/client2.cpp b/client2.cpp
index 999d5fe..42b4d05 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -360,8 +360,9 @@ void calc_undistort_maps(float px_per_deg, int width, int height, Mat& map1, Mat
camera_matrix2.at<float>(1,2)=height/2.; //cy
float px_per_rad = px_per_deg * PI / 180.;
+ float k = px_per_rad / width;
- 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);
+ Matx<float,1,5> dist_coeffs(-k*k/3.f, k*k*k*k/5.f, 0.f, 0.f, -k*k*k*k*k*k/7.f);
initUndistortRectifyMap(camera_matrix, dist_coeffs, Mat(), camera_matrix2, Size(width,height), CV_32FC1, map1, map2);
}
@@ -420,6 +421,21 @@ int init_ohmd(ohmd_context** ctx_, ohmd_device** hmd_)
}
+float deadzone(float val, float dead)
+{
+ if (val < 0.)
+ {
+ if (val < -dead) val+=dead;
+ else val=0.;
+ }
+ else
+ {
+ if (val > dead) val-=dead;
+ else val=0.;
+ }
+ return val;
+}
+
int main(int argc, const char** argv)
{
@@ -509,7 +525,7 @@ int main(int argc, const char** argv)
navdata_t navdata;
Mat map1(Size(width,height), CV_32FC1), map2(Size(width,height), CV_32FC1);
- calc_undistort_maps(80/1280., 1280,720, map1, map2);
+ calc_undistort_maps(80, 1280,720, map1, map2);
float diag = sqrt(1280*1280+720*720);
float px_per_deg = diag / 92.;
@@ -619,13 +635,17 @@ int main(int argc, const char** argv)
double yaw_diff = fixup_range( ringbuf_yaw_cam.get() - ringbuf_yaw_sensor.get(), -180, 180);
double pitch_diff = ringbuf_pitch_cam.get() + ringbuf_pitch_sensor.get();
double roll_diff = fixup_angle(ringbuf_roll_cam.get() - (-ringbuf_roll_sensor.get()));
+
+ yaw_diff = deadzone(yaw_diff, 1.0);
+ pitch_diff = deadzone(pitch_diff, 1.0);
+ roll_diff = deadzone(roll_diff, 1.0);
//if (fabs(xdiff) < px_per_deg) xdiff = 0.0;
//if (fabs(ydiff) < px_per_deg) ydiff = 0.0;
//if (fabs(adiff) < 2) adiff = 0.0;
- yaw_diff*=0.02;
- pitch_diff*=0.02;
+ yaw_diff*=0.1;
+ pitch_diff*=0.1;
roll_diff*=0.5;
yaw_cam = fixup_range(yaw_cam - yaw_diff, -180, 180);
pitch_cam = pitch_cam - pitch_diff;