diff --git a/Main/src/main/java/com/chameleonvision/vision/camera/CameraValues.java b/Main/src/main/java/com/chameleonvision/vision/camera/CameraValues.java index 2190c139b..a6d11e8a1 100644 --- a/Main/src/main/java/com/chameleonvision/vision/camera/CameraValues.java +++ b/Main/src/main/java/com/chameleonvision/vision/camera/CameraValues.java @@ -37,10 +37,11 @@ public class CameraValues { HorizontalView = FastMath.atan(FastMath.tan(DiagonalView / 2) * (HorizontalRatio / DiagonalView)) * 2; VerticalView = FastMath.atan(FastMath.tan(DiagonalView/2) * (VerticalRatio / DiagonalView)) * 2; HorizontalFocalLength = ImageWidth / (2 * FastMath.tan(HorizontalView /2)); - VerticalFocalLength = ImageWidth / (2 * FastMath.tan(VerticalView /2)); + VerticalFocalLength = ImageHeight / (2 * FastMath.tan(VerticalView /2)); } public double CalculatePitch(double PixelY, double centerY){ - return (FastMath.toDegrees((FastMath.atan(PixelY - centerY) / VerticalFocalLength)) * -1); + double pitch = (FastMath.toDegrees((FastMath.atan(PixelY - centerY) / VerticalFocalLength))); + return (pitch * -1); } public double CalculateYaw(double PixelX, double centerX){ return FastMath.toDegrees(FastMath.atan(PixelX - centerX) / HorizontalFocalLength); diff --git a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java index 58dc77031..164d16e17 100644 --- a/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java +++ b/Main/src/main/java/com/chameleonvision/vision/process/CameraProcess.java @@ -120,6 +120,9 @@ public class CameraProcess implements Runnable { if (currentPipeline == null) { return pipelineResult; } + if (!currentPipeline.orientation.equals("Normal")){ + Core.flip(inputImage,inputImage,-1); + } if (ntDriverModeEntry.getBoolean(false)){ inputImage.copyTo(outputImage); return pipelineResult; @@ -149,9 +152,9 @@ public class CameraProcess implements Runnable { } else { pipelineResult.CalibratedX = (finalRect.center.y - currentPipeline.B) / currentPipeline.M; pipelineResult.CalibratedY = finalRect.center.x * currentPipeline.M + currentPipeline.B; - pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); - pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); } + pipelineResult.Pitch = camera.getCamVals().CalculatePitch(finalRect.center.y, pipelineResult.CalibratedY); + pipelineResult.Yaw = camera.getCamVals().CalculateYaw(finalRect.center.x, pipelineResult.CalibratedX); // TODO Send pitch yaw distance and Raw Point using websockets to client for calib calc drawContour(outputImage, finalRect); }