diff --git a/build.gradle b/build.gradle
index eb3e7f8..c7c139a 100644
--- a/build.gradle
+++ b/build.gradle
@@ -24,7 +24,7 @@ repositories {
 }
 
 dependencies {
-    compile 'com.fasterxml.jackson.core:jackson-databind:2.5.3'
+    compile "com.fasterxml.jackson.module:jackson-module-kotlin:2.8.7"
     compile 'com.neuronrobotics:nrjavaserial:3.11.0'
     compile 'com.pi4j:pi4j-core:1.0'
     compile 'commons-cli:commons-cli:1.3.1'
diff --git a/src/main/java/com/easternedgerobotics/rov/Topside.java b/src/main/java/com/easternedgerobotics/rov/Topside.java
index cb7bae9..4939e4e 100644
--- a/src/main/java/com/easternedgerobotics/rov/Topside.java
+++ b/src/main/java/com/easternedgerobotics/rov/Topside.java
@@ -2,6 +2,7 @@
 
 import com.easternedgerobotics.rov.config.CameraCalibrationConfig;
 import com.easternedgerobotics.rov.config.Config;
+import com.easternedgerobotics.rov.config.DistanceCalculatorConfig;
 import com.easternedgerobotics.rov.config.JoystickConfig;
 import com.easternedgerobotics.rov.config.LaunchConfig;
 import com.easternedgerobotics.rov.config.SliderConfig;
@@ -23,6 +24,7 @@
 import com.easternedgerobotics.rov.io.arduino.ArduinoPort;
 import com.easternedgerobotics.rov.io.joystick.JoystickController;
 import com.easternedgerobotics.rov.io.joystick.LogitechExtremeJoystickSource;
+import com.easternedgerobotics.rov.math.DistanceCalculator;
 import com.easternedgerobotics.rov.value.CameraCalibrationValue;
 import com.easternedgerobotics.rov.value.MotionPowerValue;
 import com.easternedgerobotics.rov.video.CameraCalibration;
@@ -115,16 +117,23 @@ public void init() throws SocketException, UnknownHostException {
             configSource.getConfig("videoDecoder",
                 VideoDecoderConfig.class));
 
+        final ValueStore<CameraCalibrationValue> cameraCalibrationStore = ValueStore.of(
+            CameraCalibrationValue.class, config.preferencesHome());
+
         cameraCalibration = new CameraCalibration(
             eventPublisher,
             configSource.getConfig("cameraCalibration", CameraCalibrationConfig.class),
-            ValueStore.of(CameraCalibrationValue.class, config.preferencesHome()),
+            cameraCalibrationStore,
             Schedulers.newThread());
 
         fileReceiver = new TcpFileReceiver(
             launchConfig.fileReceiverPort(),
             launchConfig.fileReceiverSocketBacklog());
 
+        final DistanceCalculator distanceCalculator = new DistanceCalculator(
+            configSource.getConfig("distanceCalculator", DistanceCalculatorConfig.class),
+            cameraCalibrationStore);
+
         viewLoader = new ViewLoader(MainView.class, "Control Software", new HashMap<Class<?>, Object>() {
             {
                 put(EventPublisher.class, eventPublisher);
@@ -132,6 +141,7 @@ public void init() throws SocketException, UnknownHostException {
                 put(EmergencyStopController.class, emergencyStopController);
                 put(VideoDecoder.class, videoDecoder);
                 put(CameraCalibration.class, cameraCalibration);
+                put(DistanceCalculator.class, distanceCalculator);
             }
         });
 
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorView.java b/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorView.java
index 2c4f2f4..5d9f93f 100644
--- a/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorView.java
+++ b/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorView.java
@@ -1,8 +1,13 @@
 package com.easternedgerobotics.rov.fx;
 
+import com.easternedgerobotics.rov.fx.distance.AxisNode;
+import com.easternedgerobotics.rov.fx.distance.TextNode;
+
 import javafx.geometry.Insets;
 import javafx.scene.Parent;
 import javafx.scene.control.Button;
+import javafx.scene.image.Image;
+import javafx.scene.image.ImageView;
 import javafx.scene.layout.Border;
 import javafx.scene.layout.BorderPane;
 import javafx.scene.layout.BorderStroke;
@@ -10,11 +15,14 @@
 import javafx.scene.layout.BorderWidths;
 import javafx.scene.layout.CornerRadii;
 import javafx.scene.layout.HBox;
+import javafx.scene.layout.StackPane;
 import javafx.scene.layout.VBox;
 import javafx.scene.paint.Color;
 
+import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
+import java.util.concurrent.atomic.AtomicReference;
 import javax.inject.Inject;
 
 public final class DistanceCalculatorView implements View {
@@ -24,11 +32,17 @@ public final class DistanceCalculatorView implements View {
 
     static final double RADIUS = 3;
 
+    static final int SELECTION_WIDTH = 300;
+
     final BorderPane galleryBorderPane = new BorderPane();
 
+    final BorderPane calculatorBorderPane = new BorderPane();
+
+    final HBox mainPanel = new HBox();
+
     final HBox buttonPanel = new HBox();
 
-    final VBox mainPanel = new VBox();
+    final VBox imageSelectionPanel = new VBox();
 
     final BorderPane borderPane = new BorderPane();
 
@@ -36,6 +50,16 @@ public final class DistanceCalculatorView implements View {
 
     final Button captureB = new Button("Capture B");
 
+    final Button calculateButton = new Button("Calculate");
+
+    final ImageView imageView = new ImageView();
+
+    final StackPane imageStack = new StackPane();
+
+    final AtomicReference<AxisNode> axisNode = new AtomicReference<>(new AxisNode());
+
+    final List<TextNode> imagePoints = new ArrayList<>();
+
     @Inject
     public DistanceCalculatorView() {
         final List<Button> buttons = Arrays.asList(captureA, captureB);
@@ -44,19 +68,43 @@ public DistanceCalculatorView() {
             button.prefWidthProperty().bind(buttonPanel.widthProperty().divide(buttons.size()));
         }
         buttonPanel.getChildren().addAll(buttons);
+        calculateButton.setPrefHeight(BUTTON_HEIGHT);
+        calculateButton.prefWidthProperty().bind(buttonPanel.widthProperty());
 
         galleryBorderPane.setBorder(new Border(new BorderStroke(Color.GRAY,
             BorderStrokeStyle.SOLID, new CornerRadii(RADIUS), BorderWidths.DEFAULT)));
 
-        galleryBorderPane.prefHeightProperty().bind(mainPanel.heightProperty().subtract(BUTTON_HEIGHT));
+        galleryBorderPane.prefHeightProperty().bind(imageSelectionPanel.heightProperty().subtract(BUTTON_HEIGHT * 2));
+
+        imageSelectionPanel.setPadding(new Insets(PADDING, PADDING, PADDING, PADDING));
+        imageSelectionPanel.setSpacing(PADDING);
+        imageSelectionPanel.getChildren().addAll(buttonPanel, calculateButton, galleryBorderPane);
+        imageSelectionPanel.setMaxWidth(SELECTION_WIDTH);
+        imageSelectionPanel.setMinWidth(SELECTION_WIDTH);
 
-        mainPanel.setPadding(new Insets(PADDING, PADDING, PADDING, PADDING));
-        mainPanel.setSpacing(PADDING);
-        mainPanel.getChildren().addAll(buttonPanel, galleryBorderPane);
+        imageView.setStyle("-fx-background-color: BLACK");
+        imageView.setPreserveRatio(true);
+        imageView.setSmooth(true);
+        imageView.setCache(true);
+
+        calculatorBorderPane.setCenter(imageStack);
+        calculatorBorderPane.setStyle("-fx-background-color: BLACK");
+        calculatorBorderPane.setBorder(new Border(new BorderStroke(Color.GRAY,
+            BorderStrokeStyle.SOLID, new CornerRadii(RADIUS), BorderWidths.DEFAULT)));
+        calculatorBorderPane.prefWidthProperty().bind(mainPanel.widthProperty().subtract(SELECTION_WIDTH));
+        calculatorBorderPane.prefHeightProperty().bind(mainPanel.heightProperty());
+
+        mainPanel.getChildren().addAll(calculatorBorderPane, imageSelectionPanel);
 
         borderPane.setCenter(mainPanel);
     }
 
+    public void setImage(final Image image) {
+        imageView.setFitWidth(borderPane.getWidth() - SELECTION_WIDTH);
+        imageView.setFitHeight(borderPane.getHeight());
+        imageView.setImage(image);
+    }
+
     @Override
     public Parent getParent() {
         return borderPane;
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorViewController.java b/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorViewController.java
index 91913d9..a3ced4b 100644
--- a/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorViewController.java
+++ b/src/main/java/com/easternedgerobotics/rov/fx/DistanceCalculatorViewController.java
@@ -1,17 +1,43 @@
 package com.easternedgerobotics.rov.fx;
 
+import com.easternedgerobotics.rov.config.CameraCalibrationConfig;
 import com.easternedgerobotics.rov.config.Configurable;
 import com.easternedgerobotics.rov.config.DistanceCalculatorConfig;
+import com.easternedgerobotics.rov.fx.distance.AxisNode;
+import com.easternedgerobotics.rov.fx.distance.ShapeScene;
+import com.easternedgerobotics.rov.fx.distance.TextNode;
 import com.easternedgerobotics.rov.io.FileUtil;
+import com.easternedgerobotics.rov.math.DistanceCalculator;
+import com.easternedgerobotics.rov.value.AxisValue;
+import com.easternedgerobotics.rov.value.PointValue;
 import com.easternedgerobotics.rov.video.CameraCalibration;
 
+import javafx.event.EventType;
+import javafx.scene.control.ContextMenu;
+import javafx.scene.control.MenuItem;
+import javafx.scene.image.Image;
+import javafx.scene.input.MouseButton;
+import javafx.scene.input.MouseEvent;
+import javafx.scene.paint.Color;
+import javafx.scene.shape.Circle;
+import org.pmw.tinylog.Logger;
 import rx.Observable;
+import rx.javafx.sources.Change;
 import rx.javafx.sources.Flag;
 import rx.javafx.sources.ListChange;
 import rx.observables.JavaFxObservable;
 import rx.subscriptions.CompositeSubscription;
 
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardCopyOption;
 import java.util.Arrays;
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+import java.util.stream.Collectors;
 import javax.inject.Inject;
 
 public final class DistanceCalculatorViewController implements ViewController {
@@ -20,15 +46,20 @@ public final class DistanceCalculatorViewController implements ViewController {
      */
     private final CompositeSubscription subscriptions = new CompositeSubscription();
 
+    /**
+     * Holds onto subscriptions of the window size.
+     */
+    private final CompositeSubscription currentDistanceSubscriptions = new CompositeSubscription();
+
     /**
      * The label of the open image option.
      */
-    static final String OPEN_ACTION = "Open";
+    private static final String OPEN_ACTION = "Open";
 
     /**
      * The label of the delete image option.
      */
-    static final String DELETE_ACTION = "Delete";
+    private static final String DELETE_ACTION = "Delete";
 
     /**
      * The view controlled by this instance.
@@ -46,9 +77,19 @@ public final class DistanceCalculatorViewController implements ViewController {
     private final CameraCalibration cameraCalibration;
 
     /**
-     * The config associated with the distance calculator.
+     * Holds the configuration of the camera calibration.
+     */
+    private final CameraCalibrationConfig cameraCalibrationConfig;
+
+    /**
+     * Performs the distance calculations on a set of image points.
+     */
+    private final DistanceCalculator distanceCalculator;
+
+    /**
+     * Holds the configuration of the distance calculator.
      */
-    private final DistanceCalculatorConfig config;
+    private final DistanceCalculatorConfig distanceCalculatorConfig;
 
     /**
      * Control the distance calculator using this controller.
@@ -56,19 +97,25 @@ public final class DistanceCalculatorViewController implements ViewController {
      * @param view The view controlled by this instance.
      * @param gallery A gallery of undistorted images.
      * @param cameraCalibration The calibration object used to create undistorted images.
-     * @param config The config associated with the distance calculator.
+     * @param cameraCalibrationConfig Holds the configuration of the camera calibration.
+     * @param distanceCalculator The instance used to calculate distances
+     * @param distanceCalculatorConfig Holds the configuration of the distance calculator.
      */
     @Inject
     public DistanceCalculatorViewController(
         final DistanceCalculatorView view,
         final GalleryView gallery,
         final CameraCalibration cameraCalibration,
-        @Configurable("distanceCalculator") final DistanceCalculatorConfig config
+        @Configurable("cameraCalibration") final CameraCalibrationConfig cameraCalibrationConfig,
+        final DistanceCalculator distanceCalculator,
+        @Configurable("distanceCalculator") final DistanceCalculatorConfig distanceCalculatorConfig
     ) {
         this.view = view;
         this.gallery = gallery;
         this.cameraCalibration = cameraCalibration;
-        this.config = config;
+        this.cameraCalibrationConfig = cameraCalibrationConfig;
+        this.distanceCalculator = distanceCalculator;
+        this.distanceCalculatorConfig = distanceCalculatorConfig;
     }
 
     @Override
@@ -81,25 +128,113 @@ public void onCreate() {
             .map(ListChange::getValue)
             .flatMap(iv -> iv.doubleClickAction(OPEN_ACTION)
                 .takeUntil(iv.menuAction(DELETE_ACTION))
-                .doOnCompleted(() -> iv.getPath().toFile().delete())
-                .map(p -> iv))
-            .subscribe(GalleryImageView::fullscreen));
+                .doOnCompleted(() -> iv.getPath().toFile().delete()))
+            .subscribe(this::setImageFromPath));
 
         gallery.actions.addAll(Arrays.asList(OPEN_ACTION, DELETE_ACTION));
-        gallery.directoryLabel.setText(config.imageDirectory());
+        gallery.directoryLabel.setText(distanceCalculatorConfig.imageDirectory());
         view.galleryBorderPane.setCenter(gallery.getParent());
 
         subscriptions.add(JavaFxObservable.valuesOf(view.captureA.pressedProperty()).filter(x -> !x).skip(1)
             .subscribe(x -> cameraCalibration.captureUndistortedImageA(
-                FileUtil.nextName(config.imageDirectory(), "undistortedA", "png"))));
+                FileUtil.nextName(distanceCalculatorConfig.imageDirectory(), "undistortedA", "png"))));
 
         subscriptions.add(JavaFxObservable.valuesOf(view.captureB.pressedProperty()).filter(x -> !x).skip(1)
             .subscribe(x -> cameraCalibration.captureUndistortedImageB(
-                FileUtil.nextName(config.imageDirectory(), "undistortedB", "png"))));
+                FileUtil.nextName(distanceCalculatorConfig.imageDirectory(), "undistortedB", "png"))));
+    }
+
+    /**
+     * Set the calculator to display an image from the following path, and resize this image if the windows changes.
+     *
+     * @param path
+     */
+    public void setImageFromPath(final Path path) {
+        currentDistanceSubscriptions.clear();
+        view.imageStack.getChildren().clear();
+        view.imageStack.getChildren().addAll(view.imageView);
+        view.imagePoints.clear();
+
+        final ShapeScene shapeScene = new ShapeScene(view.imageStack, view.imageView, currentDistanceSubscriptions);
+
+        final Observable<Number> sizeChanged = Observable.merge(
+            JavaFxObservable.changesOf(view.borderPane.getScene().widthProperty()),
+            JavaFxObservable.changesOf(view.borderPane.getScene().heightProperty())
+        ).map(Change::getNewVal).startWith(0);
+
+        // On fullscreen some component will not be fully adjusted.
+        // Delay 1 millisecond to allow value propagation.
+        currentDistanceSubscriptions.add(sizeChanged
+            .delay(1, TimeUnit.MILLISECONDS, JAVA_FX_SCHEDULER)
+            .subscribe(v -> shapeScene.draw()));
+
+        if (path.toString().contains("undistortedA") || path.toString().contains("undistortedB")) {
+            final String cameraName;
+            if (path.toString().contains("undistortedA")) {
+                cameraName = cameraCalibrationConfig.cameraAName();
+            } else {
+                cameraName = cameraCalibrationConfig.cameraBName();
+            }
+            final Observable<Boolean> calcPress = JavaFxObservable.valuesOf(view.calculateButton.pressedProperty());
+            currentDistanceSubscriptions.add(calcPress.filter(x -> !x).skip(1).subscribe(x -> {
+                final AxisValue axis = view.axisNode.get().getAxis();
+                final List<PointValue> pixelPoints = view.imagePoints
+                    .stream().map(TextNode::getPoint).collect(Collectors.toList());
+                distanceCalculator.calculate(axis, pixelPoints, cameraName).ifPresent(objectPoints -> {
+                    for (int i = 0; i < objectPoints.size(); i++) {
+                        view.imagePoints.get(i).setText(String.format("(%.2f, %.2f)",
+                            objectPoints.get(i).getX(), objectPoints.get(i).getY()));
+                    }
+                    shapeScene.draw();
+                });
+            }));
+        }
+
+        try {
+
+            final File tempFile = File.createTempFile(path.toFile().getName(), ".tmp");
+            Files.copy(path, tempFile.toPath(), StandardCopyOption.REPLACE_EXISTING);
+            final Image image = new Image(new FileInputStream(tempFile));
+            currentDistanceSubscriptions.add(sizeChanged.subscribe(v -> {
+                view.imageStack.getChildren().remove(view.imageView);
+                view.setImage(image);
+                view.imageStack.getChildren().add(0, view.imageView);
+            }));
+
+            final ContextMenu contextMenu = new ContextMenu();
+            final MenuItem axis = new MenuItem("Axis");
+            final MenuItem point = new MenuItem("Point");
+            contextMenu.getItems().addAll(axis, point);
+
+            final Observable<MouseEvent> events = JavaFxObservable.eventsOf(view.imageView, EventType.ROOT)
+                .filter(event -> event.getEventType().equals(MouseEvent.MOUSE_CLICKED))
+                .cast(MouseEvent.class)
+                .filter(event -> event.getButton().equals(MouseButton.SECONDARY))
+                .doOnNext(event -> contextMenu.show(view.imageView, event.getScreenX(), event.getScreenY()))
+                .share();
+
+            currentDistanceSubscriptions.add(JavaFxObservable.actionEventsOf(axis).withLatestFrom(events, (a, e) -> e)
+                .subscribe(event -> {
+                    final AxisNode axisNode = new AxisNode();
+                    view.axisNode.set(axisNode);
+                    shapeScene.add(axisNode, event.getX(), event.getY());
+                }));
+
+            currentDistanceSubscriptions.add(JavaFxObservable.actionEventsOf(point).withLatestFrom(events, (a, e) -> e)
+                .subscribe(event -> {
+                    final TextNode imagePoint = new TextNode(new Circle(7, Color.CHARTREUSE));
+                    view.imagePoints.add(imagePoint);
+                    shapeScene.add(imagePoint, event.getX(), event.getY());
+                }));
+
+        } catch (final IOException e) {
+            Logger.error(e);
+        }
     }
 
     @Override
     public void onDestroy() {
         subscriptions.unsubscribe();
+        currentDistanceSubscriptions.unsubscribe();
     }
 }
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java b/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java
index 6c5e348..eb820e3 100644
--- a/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java
+++ b/src/main/java/com/easternedgerobotics/rov/fx/GalleryViewController.java
@@ -61,6 +61,7 @@ private void updateDirectory(final ObservableValue<? extends String> value, fina
         subscriptions.clear();
         subscriptions.add(DirectoryUtil.observe(Paths.get(curr))
             .subscribeOn(Schedulers.newThread())
+            .delay(1, TimeUnit.SECONDS)
             .subscribe(this::onImagePathChanged));
     }
 
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/distance/AxisNode.java b/src/main/java/com/easternedgerobotics/rov/fx/distance/AxisNode.java
new file mode 100644
index 0000000..a71763c
--- /dev/null
+++ b/src/main/java/com/easternedgerobotics/rov/fx/distance/AxisNode.java
@@ -0,0 +1,146 @@
+package com.easternedgerobotics.rov.fx.distance;
+
+import com.easternedgerobotics.rov.value.AxisValue;
+import com.easternedgerobotics.rov.value.PointValue;
+
+import javafx.scene.paint.Color;
+import javafx.scene.shape.Circle;
+import javafx.scene.shape.Line;
+import javafx.scene.shape.Shape;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public final class AxisNode implements ShapeNode {
+    private static final int CIRCLE_SIZE = 7;
+
+    private static final int STROKE_WIDTH = 5;
+
+    private static final double XY_AXIS_OFFSET = 0.10;
+
+    private static final double Z_AXIS_OFFSET = 0.10;
+
+    private final Shape shape;
+
+    private final Line xLine;
+
+    private final Line yLine;
+
+    private final Line zLine;
+
+    private final List<ShapeNode> children = new ArrayList<>();
+
+    private final TextNode xNode;
+
+    private final TextNode yNode;
+
+    private final TextNode zNode;
+
+    private volatile double x;
+
+    private volatile double y;
+
+    public AxisNode() {
+        shape = new Circle(CIRCLE_SIZE, Color.RED);
+        xNode = new TextNode(new Circle(CIRCLE_SIZE, Color.BLUE));
+        yNode = new TextNode(new Circle(CIRCLE_SIZE, Color.GREEN));
+        zNode = new TextNode(new Circle(CIRCLE_SIZE, Color.ORANGE));
+        xNode.setText("X Axis");
+        yNode.setText("Y Axis");
+        zNode.setText("Z Axis");
+        children.add(xNode);
+        children.add(yNode);
+        children.add(zNode);
+        xNode.nodeMoved(XY_AXIS_OFFSET, -XY_AXIS_OFFSET);
+        yNode.nodeMoved(-XY_AXIS_OFFSET, -XY_AXIS_OFFSET);
+        zNode.nodeMoved(0.00, -Z_AXIS_OFFSET);
+        xLine = new Line();
+        yLine = new Line();
+        zLine = new Line();
+        xLine.setStrokeWidth(STROKE_WIDTH);
+        yLine.setStrokeWidth(STROKE_WIDTH);
+        zLine.setStrokeWidth(STROKE_WIDTH);
+        xLine.setStroke(Color.BLUE);
+        yLine.setStroke(Color.GREEN);
+        zLine.setStroke(Color.ORANGE);
+    }
+
+    @Override
+    public double getX() {
+        return x;
+    }
+
+    @Override
+    public double getY() {
+        return y;
+    }
+
+    @Override
+    public void nodeMoved(final double dx, final double dy) {
+        x += dx;
+        y += dy;
+        children.forEach(n -> n.nodeMoved(dx, dy));
+    }
+
+    @Override
+    public Shape getHandleShape() {
+        return shape;
+    }
+
+    @Override
+    public List<Shape> getDrawnShapes() {
+        return Arrays.asList(xLine, yLine, zLine, shape);
+    }
+
+    @Override
+    public List<ShapeNode> getChildren() {
+        return children;
+    }
+
+    @Override
+    public void toFront() {
+        xLine.toFront();
+        yLine.toFront();
+        zLine.toFront();
+        shape.toFront();
+        children.forEach(ShapeNode::toFront);
+    }
+
+    @Override
+    public void draw(final double sceneWidth, final double sceneHeight) {
+        if (shape != null) {
+            final double x0 = x * sceneWidth;
+            final double y0 = y * sceneHeight;
+            shape.setTranslateX(x0);
+            shape.setTranslateY(y0);
+            xLine.setStartX(x0);
+            yLine.setStartX(x0);
+            zLine.setStartX(x0);
+            xLine.setStartY(y0);
+            yLine.setStartY(y0);
+            zLine.setStartY(y0);
+            xLine.setEndX(xNode.getX() * sceneWidth);
+            yLine.setEndX(yNode.getX() * sceneWidth);
+            zLine.setEndX(zNode.getX() * sceneWidth);
+            xLine.setEndY(xNode.getY() * sceneHeight);
+            yLine.setEndY(yNode.getY() * sceneHeight);
+            zLine.setEndY(zNode.getY() * sceneHeight);
+            xLine.setTranslateX((x0 + xNode.getX() * sceneWidth) / 2);
+            yLine.setTranslateX((x0 + yNode.getX() * sceneWidth) / 2);
+            zLine.setTranslateX((x0 + zNode.getX() * sceneWidth) / 2);
+            xLine.setTranslateY((y0 + xNode.getY() * sceneHeight) / 2);
+            yLine.setTranslateY((y0 + yNode.getY() * sceneHeight) / 2);
+            zLine.setTranslateY((y0 + zNode.getY() * sceneHeight) / 2);
+        }
+    }
+
+    /**
+     * Convert this node into a value object representation.
+     *
+     * @return a new axis value.
+     */
+    public AxisValue getAxis() {
+        return new AxisValue(new PointValue(x, y), xNode.getPoint(), yNode.getPoint(), zNode.getPoint());
+    }
+}
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeNode.java b/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeNode.java
new file mode 100644
index 0000000..b31c5bc
--- /dev/null
+++ b/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeNode.java
@@ -0,0 +1,63 @@
+package com.easternedgerobotics.rov.fx.distance;
+
+import javafx.scene.shape.Shape;
+
+import java.util.List;
+
+public interface ShapeNode {
+    /**
+     * Get the horizontal position in percentage from the center of the image to the edge of the scene.
+     *
+     * @return the x value
+     */
+    double getX();
+
+    /**
+     * Get the vertical position in percentage from the center of the image to the edge of the scene.
+     *
+     * @return the y value
+     */
+    double getY();
+
+    /**
+     * Update the location of the node by the given offset.
+     *
+     * @param dx percentage change in horizontal
+     * @param dy percentage change in vertical
+     */
+    void nodeMoved(final double dx, final double dy);
+
+    /**
+     * Get the shape which is used to interact with the node.
+     *
+     * @return the handle
+     */
+    Shape getHandleShape();
+
+    /**
+     * Get the shapes used to represent this node on screen.
+     *
+     * @return the drawn shapes of the node.
+     */
+    List<Shape> getDrawnShapes();
+
+    /**
+     * The nodes which should be updated when this node is updated.
+     *
+     * @return the children shapes.
+     */
+    List<ShapeNode> getChildren();
+
+    /**
+     * Bring the shapes in the node to the front of its container.
+     */
+    void toFront();
+
+    /**
+     * Update the x and y position of the contained shape to be displayed in the given screen size.
+     *
+     * @param sceneWidth the target screen width.
+     * @param sceneHeight the target screen height.
+     */
+    void draw(final double sceneWidth, final double sceneHeight);
+}
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeScene.java b/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeScene.java
new file mode 100644
index 0000000..e2b9694
--- /dev/null
+++ b/src/main/java/com/easternedgerobotics/rov/fx/distance/ShapeScene.java
@@ -0,0 +1,104 @@
+package com.easternedgerobotics.rov.fx.distance;
+
+import javafx.event.EventType;
+import javafx.scene.Node;
+import javafx.scene.input.MouseButton;
+import javafx.scene.input.MouseEvent;
+import javafx.scene.layout.StackPane;
+import rx.observables.JavaFxObservable;
+import rx.subscriptions.CompositeSubscription;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public final class ShapeScene {
+    /**
+     * A list of nodes within the scene which have no parent.
+     */
+    private final List<ShapeNode> children = new ArrayList<>();
+
+    /**
+     * The stack used to display these nodes.
+     */
+    private final StackPane stack;
+
+    /**
+     * The bounds used to locate node relative positions.
+     */
+    private final Node bounds;
+
+    /**
+     * Holds all subscriptions made by this scene.
+     */
+    private final CompositeSubscription subscriptions;
+
+    /**
+     * Construct a controller for the elements in a shape node scene.
+     * Handles movement of the scene nodes and redraws the scene when required.
+     *
+     * @param stack the container of the node shapes.
+     * @param bounds a node to use as the bounds of the scene.
+     * @param subscriptions the holder of subscriptions created by this scene.
+     */
+    public ShapeScene(final StackPane stack, final Node bounds, final CompositeSubscription subscriptions) {
+        this.stack = stack;
+        this.bounds = bounds;
+        this.subscriptions = subscriptions;
+    }
+
+    /**
+     * Add a new node to the scene and manage movement and drawing.
+     *
+     * @param node the node to be managed.
+     * @param containerX the starting x position of the node in the container (stack).
+     * @param containerY the starting y position of the node in the container (stack).
+     */
+    public void add(final ShapeNode node, final double containerX, final double containerY) {
+        children.add(node);
+        node.nodeMoved(
+            (containerX / bounds.getBoundsInParent().getWidth()) - (1.0 / 2.0),
+            (containerY / bounds.getBoundsInParent().getHeight()) - (1.0 / 2.0));
+        init(node);
+        draw(node);
+    }
+
+    /**
+     * Set up movement subscriptions for the node and add drawables to the scene.
+     *
+     * @param node the node to be initialized.
+     */
+    private void init(final ShapeNode node) {
+        subscriptions.add(JavaFxObservable
+            .eventsOf(node.getHandleShape(), EventType.ROOT)
+            .filter(e -> e.getEventType().equals(MouseEvent.MOUSE_DRAGGED))
+            .cast(MouseEvent.class)
+            .filter(e -> e.getButton().equals(MouseButton.PRIMARY))
+            .subscribe(e -> {
+                node.toFront();
+                node.nodeMoved(
+                    e.getX() / bounds.getBoundsInParent().getWidth(),
+                    e.getY() / bounds.getBoundsInParent().getHeight());
+                draw();
+            })
+        );
+        stack.getChildren().addAll(node.getDrawnShapes());
+        node.getChildren().forEach(this::init);
+    }
+
+    /**
+     * Redraw the nodes within in the scene.
+     */
+    public void draw() {
+        children.forEach(this::draw);
+    }
+
+    /**
+     * Redraw a node and all of its children within a scene.
+     *
+     * @param node the node to be redrawn.
+     */
+    private void draw(final ShapeNode node) {
+        node.draw(bounds.getBoundsInParent().getWidth(), bounds.getBoundsInParent().getHeight());
+        node.getChildren().forEach(this::draw);
+    }
+}
diff --git a/src/main/java/com/easternedgerobotics/rov/fx/distance/TextNode.java b/src/main/java/com/easternedgerobotics/rov/fx/distance/TextNode.java
new file mode 100644
index 0000000..8f65c2c
--- /dev/null
+++ b/src/main/java/com/easternedgerobotics/rov/fx/distance/TextNode.java
@@ -0,0 +1,102 @@
+package com.easternedgerobotics.rov.fx.distance;
+
+import com.easternedgerobotics.rov.value.PointValue;
+
+import javafx.scene.paint.Color;
+import javafx.scene.shape.Shape;
+import javafx.scene.text.Font;
+import javafx.scene.text.FontWeight;
+import javafx.scene.text.Text;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public final class TextNode implements ShapeNode {
+    private static final int FONT_SIZE = 20;
+
+    private static final int FONT_OFFSET = 20;
+
+    private final Shape shape;
+
+    private final Text text;
+
+    private final List<ShapeNode> children = new ArrayList<>();
+
+    private volatile double x;
+
+    private volatile double y;
+
+    public TextNode(final Shape shape) {
+        this.shape = shape;
+        text = new Text();
+        text.setFont(Font.font("Verdana", FontWeight.BOLD, FONT_SIZE));
+        text.setStroke(Color.BLACK);
+        text.setStrokeWidth(1);
+        text.setFill(Color.WHITE);
+    }
+
+    @Override
+    public void nodeMoved(final double dx, final double dy) {
+        x += dx;
+        y += dy;
+    }
+
+    @Override
+    public double getX() {
+        return x;
+    }
+
+    @Override
+    public double getY() {
+        return y;
+    }
+
+    @Override
+    public Shape getHandleShape() {
+        return shape;
+    }
+
+    @Override
+    public List<Shape> getDrawnShapes() {
+        return Arrays.asList(shape, text);
+    }
+
+    public List<ShapeNode> getChildren() {
+        return children;
+    }
+
+    @Override
+    public void toFront() {
+        shape.toFront();
+        text.toFront();
+    }
+
+    @Override
+    public void draw(final double sceneWidth, final double sceneHeight) {
+        if (shape != null) {
+            shape.setTranslateX(x * sceneWidth);
+            shape.setTranslateY(y * sceneHeight);
+            text.setTranslateX(x * sceneWidth);
+            text.setTranslateY((y * sceneHeight) - FONT_OFFSET);
+        }
+    }
+
+    /**
+     * Set the text displayed next to this node.
+     *
+     * @param label the new text.
+     */
+    public void setText(final String label) {
+        text.setText(label);
+    }
+
+    /**
+     * Convert this node into a value object representation.
+     *
+     * @return a new point value.
+     */
+    public PointValue getPoint() {
+        return new PointValue(x, y);
+    }
+}
diff --git a/src/main/java/com/easternedgerobotics/rov/io/ValueStore.java b/src/main/java/com/easternedgerobotics/rov/io/ValueStore.java
index 240aa05..30cdcad 100644
--- a/src/main/java/com/easternedgerobotics/rov/io/ValueStore.java
+++ b/src/main/java/com/easternedgerobotics/rov/io/ValueStore.java
@@ -1,6 +1,8 @@
 package com.easternedgerobotics.rov.io;
 
+import com.fasterxml.jackson.databind.DeserializationFeature;
 import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.module.kotlin.KotlinModule;
 import org.pmw.tinylog.Logger;
 
 import java.io.IOException;
@@ -18,13 +20,18 @@ public final class ValueStore<V>  {
 
     private final Map<Object, V> values = new HashMap<>();
 
+    private final ObjectMapper mapper;
+
     public static <V> ValueStore<V> of(final Class<V> clazz, final String preferencesHome) {
-        return new ValueStore<>(clazz, Preferences.userRoot().node(preferencesHome));
+        final ObjectMapper mapper = new ObjectMapper().registerModule(new KotlinModule());
+        mapper.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
+        return new ValueStore<>(clazz, Preferences.userRoot().node(preferencesHome), mapper);
     }
 
-    private ValueStore(final Class<V> clazz, final Preferences preferences) {
+    private ValueStore(final Class<V> clazz, final Preferences preferences, final ObjectMapper mapper) {
         this.clazz = clazz;
         this.preferences = preferences;
+        this.mapper = mapper;
     }
 
     public Optional<V> get(final Object key) {
@@ -36,7 +43,7 @@ public Optional<V> get(final Object key) {
                 return null;
             }
             try {
-                return new ObjectMapper().readValue(valueStr, clazz);
+                return mapper.readValue(valueStr, clazz);
             } catch (final IOException e) {
                 Logger.warn("The value {} could not be parsed from '{}': {}", valueName, valueStr, e);
                 return null;
@@ -48,7 +55,7 @@ public void set(final Object key, final V value) {
         values.put(key, value);
         final String valueName = String.format(NAME_FORMAT, clazz.getName(), key);
         try {
-            preferences.put(valueName, new ObjectMapper().writeValueAsString(value));
+            preferences.put(valueName, mapper.writeValueAsString(value));
         } catch (final IOException e) {
             Logger.warn("The value {} could not be saved to {}: {}", value, valueName, e);
         }
diff --git a/src/main/java/com/easternedgerobotics/rov/math/DistanceCalculator.java b/src/main/java/com/easternedgerobotics/rov/math/DistanceCalculator.java
index c865cc0..5ca42c0 100644
--- a/src/main/java/com/easternedgerobotics/rov/math/DistanceCalculator.java
+++ b/src/main/java/com/easternedgerobotics/rov/math/DistanceCalculator.java
@@ -1,11 +1,249 @@
 package com.easternedgerobotics.rov.math;
 
 import com.easternedgerobotics.rov.config.DistanceCalculatorConfig;
+import com.easternedgerobotics.rov.io.ValueStore;
+import com.easternedgerobotics.rov.value.AxisValue;
+import com.easternedgerobotics.rov.value.CameraCalibrationValue;
+import com.easternedgerobotics.rov.value.PointValue;
+
+import org.bytedeco.javacpp.indexer.DoubleIndexer;
+import org.bytedeco.javacpp.opencv_calib3d;
+import org.bytedeco.javacpp.opencv_core;
+import org.bytedeco.javacpp.opencv_core.Mat;
+
+import java.util.List;
+import java.util.Optional;
+import java.util.stream.Collectors;
+
+public final class DistanceCalculator {
+    private static final double X_START = 0.1;
+
+    private static final double X_END = 1.9;
+
+    private static final double X_DELTA = 0.001;
 
-public class DistanceCalculator {
     private final DistanceCalculatorConfig config;
 
-    public DistanceCalculator(final DistanceCalculatorConfig config) {
+    private final ValueStore<CameraCalibrationValue> store;
+
+    /**
+     * Create a distance calculator which is able to determine the ground plane of an image
+     * and map image points onto this ground plane.
+     *
+     * @param config
+     * @param store
+     */
+    public DistanceCalculator(
+        final DistanceCalculatorConfig config,
+        final ValueStore<CameraCalibrationValue> store
+    ) {
         this.config = config;
+        this.store = store;
+    }
+
+    @SuppressWarnings({"checkstyle:LocalFinalVariableName", "checkstyle:MethodLength"})
+    public Optional<List<PointValue>> calculate(
+        final AxisValue axis,
+        final List<PointValue> points,
+        final String cameraName
+    ) {
+        return store.get(cameraName).map(calibration -> {
+            /**
+             * Construct the initial axis image to object map.
+             * Used by solve PnP to figure out projections from object plane to image plane.
+             */
+            final double w = calibration.getWidth();
+            final double h = calibration.getHeight();
+            final double[][] axisPixels = {
+                {(w / 2) + w * axis.getOrigin().getX(), (h / 2) +  h * axis.getOrigin().getY()},
+                {(w / 2) + w * axis.getX().getX(), (h / 2) +  h * axis.getX().getY()},
+                {(w / 2) + w * axis.getY().getX(), (h / 2) + h * axis.getY().getY()},
+            };
+            final Mat axisPixelsMat = new Mat(axisPixels.length, 1, opencv_core.CV_64FC2);
+            final DoubleIndexer axisPixelsIndexer = axisPixelsMat.createIndexer();
+            for (int j = 0; j < axisPixels.length; j++) {
+                axisPixelsIndexer.put(j, 0, 0, axisPixels[j][0]);
+                axisPixelsIndexer.put(j, 0, 1, axisPixels[j][1]);
+            }
+            final double[][] axisObject = {
+                {0, 0},
+                {1, 0},
+                {0, 1},
+            };
+            final Mat axisObjectMat = new Mat(1, axisPixels.length, opencv_core.CV_64FC3);
+            final DoubleIndexer axisObjectIndexer = axisObjectMat.createIndexer();
+            for (int j = 0; j < axisPixels.length; j++) {
+                axisObjectIndexer.put(0, j, 0, axisObject[j][0]);
+                axisObjectIndexer.put(0, j, 1, axisObject[j][1]);
+                axisObjectIndexer.put(0, j, 2, 0);
+            }
+
+            /**
+             * Construct a point on the z axis.
+             * This point when projected as a ray in the object coordinate system
+             * should intersect with the z-axis found by the PnP system.
+             * If this point does not intersect with the z-axis then we know that
+             * the scale of the object axis was not estimated correctly.
+             */
+            final Mat cameraMatrix = calibration.getCameraMatrix();
+            final Mat zAxisPoint = new Mat(3, 1, cameraMatrix.type());
+            final DoubleIndexer zAxisPointIndexer = zAxisPoint.createIndexer();
+            zAxisPointIndexer.put(0, 0, (w / 2) + w * axis.getZ().getX());
+            zAxisPointIndexer.put(1, 0, (h / 2) + h * axis.getZ().getY());
+            zAxisPointIndexer.put(2, 0, 1);
+
+            /**
+             * Begin with the assumption that the x axis is in the range of 1-2 times the size of
+             * the y axis. Edit the value in the object coordinate map to this estimate and discover
+             * a transformation for the object to image coordinates. This equation follows the format
+             *            s * Pc = M * (R * Pw + T)
+             *            s = scalar
+             *            Pc = imagePoint
+             *            Pw = objectPoint
+             *            M = intrinsicMatrix
+             *            R = rotationMatrix
+             *            T = translationVector
+             *
+             * Reverse standard mapping to figure out a mapping of image to object coordinates.
+             *           Pw = s * inv(R) * inv(M) * Pc - inv(R) * T
+             *           Pw = s * a - b
+             *
+             * When a pixel is projected into object space it creates a ray in the scene.
+             * This leaves a linear equation of:
+             *            [[Pw_0],         [[a_0],    [[b_0],
+             *             [Pw_1],  =  s *  [a_1],  -  [b_1],
+             *             [Pw_2]]          [a_2]]     [b_2]]
+             *
+             * We know that this ray must intersect at some point with the z-axis ray in object space.
+             * This means that we can constrain the system to the following:
+             *            [[0],          [[a_0],    [[b_0],
+             *             [0],   =  s *  [a_1],  -  [b_1],
+             *           [Pw_2]]          [a_2]]     [b_2]]
+             *
+             * This leaves the system with two solutions for s:
+             *              0 = s * a_0 - b_0
+             *              0 = s * a_1 - b_1
+             *
+             * For this equation to be solvable it means that:
+             *              s = b_0 / a_0
+             *              s = b_1 / a_1
+             *              b_0 / a_0 = b_1 / a_1
+             *
+             * The value of x which is the closest to being able to satisfy this equation will be considered the
+             * correct value of the object space coordinate moving forward.
+             *
+             * Start by initializing the best matches such that the first iteration overwrites them.
+             */
+            double bestDiffS = Double.MAX_VALUE;
+            double bestX = 0;
+
+            for (double x = X_START; x < X_END; x += X_DELTA) {
+                axisObjectIndexer.put(0, 2, 1, x);
+
+                final Mat rotationalVector = new Mat();
+                final Mat translationalVector = new Mat();
+                opencv_calib3d.solvePnP(
+                    axisObjectMat,
+                    axisPixelsMat,
+                    cameraMatrix,
+                    new Mat(),
+                    rotationalVector,
+                    translationalVector);
+
+                final Mat R = new Mat();
+                opencv_calib3d.Rodrigues(rotationalVector, R);
+                final Mat R_inv = R.inv().asMat();
+                final Mat T = translationalVector;
+                final Mat M = cameraMatrix;
+                final Mat M_inv = M.inv().asMat();
+
+                final Mat M_inv_Pc = new Mat();
+                opencv_core.gemm(M_inv, zAxisPoint, 1, new Mat(), 0, M_inv_Pc);
+
+                final Mat R_inv_M_inv_Pc = new Mat();
+                opencv_core.gemm(R_inv, M_inv_Pc, 1, new Mat(), 0, R_inv_M_inv_Pc);
+
+                final Mat R_inv_T = new Mat();
+                opencv_core.gemm(R_inv, T, 1, new Mat(), 0, R_inv_T);
+
+                final DoubleIndexer lhsIndexer = R_inv_M_inv_Pc.createIndexer();
+                final DoubleIndexer rhsIndexer = R_inv_T.createIndexer();
+
+                final double s0 = rhsIndexer.get(0, 0) / lhsIndexer.get(0, 0);
+                final double s1 = rhsIndexer.get(1, 0) / lhsIndexer.get(1, 0);
+
+                final double s = Math.abs(s0 - s1);
+                if (s < bestDiffS) {
+                    bestX = x;
+                    bestDiffS = s;
+                }
+            }
+
+            /**
+             * Calculate the image to object coordinate mapping using the best s from the previous algorithm.
+             */
+            axisObjectIndexer.put(0, 2, 1, bestX);
+
+            final Mat rotationalVector = new Mat();
+            final Mat translationalVector = new Mat();
+            opencv_calib3d.solvePnP(
+                axisObjectMat,
+                axisPixelsMat,
+                cameraMatrix,
+                new Mat(),
+                rotationalVector,
+                translationalVector);
+
+            final Mat R = new Mat();
+            opencv_calib3d.Rodrigues(rotationalVector, R);
+            final Mat R_inv = R.inv().asMat();
+            final Mat T = translationalVector;
+            final Mat M = cameraMatrix;
+            final Mat M_inv = M.inv().asMat();
+
+            /**
+             * Calculate the image to object coordinate mapping by assuming that
+             * the point is on the ground plane. This constrains the ray equation to:
+             *          [[Pw_0],          [[a_0],    [[b_0],
+             *           [Pw_1],   =  s *  [a_1],  -  [b_1],
+             *            [0]]             [a_2]]     [b_2]]
+             *
+             * This leaves us with a single solution for s:
+             *             0 = s * a_2 - b_2
+             *             s = b_2 / a_2
+             *
+             * Plug this value back into the first equation to solve for x and y
+             *             x = Pw_0 = (b_2 / a_2) * a_0 - b_0
+             *             y = Pw_1 = (b_2 / a_2) * a_1 - b_1
+             *
+             */
+            return points.stream().map(point -> {
+                final Mat imagePoint = new Mat(3, 1, cameraMatrix.type());
+                final DoubleIndexer imagePointIndexer = imagePoint.createIndexer();
+                imagePointIndexer.put(0, 0, (w / 2) + w * point.getX());
+                imagePointIndexer.put(1, 0, (h / 2) + h * point.getY());
+                imagePointIndexer.put(2, 0, 1);
+
+                final Mat M_inv_Pc = new Mat();
+                opencv_core.gemm(M_inv, imagePoint, 1, new Mat(), 0, M_inv_Pc);
+
+                final Mat R_inv_M_inv_Pc = new Mat();
+                opencv_core.gemm(R_inv, M_inv_Pc, 1, new Mat(), 0, R_inv_M_inv_Pc);
+
+                final Mat R_inv_T = new Mat();
+                opencv_core.gemm(R_inv, T, 1, new Mat(), 0, R_inv_T);
+
+                final DoubleIndexer lhsIndexer = R_inv_M_inv_Pc.createIndexer();
+                final DoubleIndexer rhsIndexer = R_inv_T.createIndexer();
+                final double s = rhsIndexer.get(2, 0) / lhsIndexer.get(2, 0);
+
+                final Mat Rinv_Cinv_s_P = opencv_core.multiply(s, R_inv_M_inv_Pc).asMat();
+                final Mat objectPoint = opencv_core.subtract(Rinv_Cinv_s_P, R_inv_T).asMat();
+
+                final DoubleIndexer objectPointIndexer = objectPoint.createIndexer();
+
+                return new PointValue(objectPointIndexer.get(0, 0), objectPointIndexer.get(1, 0));
+            }).collect(Collectors.toList());
+        });
     }
 }
diff --git a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java
index 75317ef..3086737 100644
--- a/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java
+++ b/src/main/java/com/easternedgerobotics/rov/video/CameraCalibration.java
@@ -103,10 +103,11 @@ public void captureUndistortedImageA(final File saveFile) {
             final File destination = FileUtil.nextName(config.cameraAPreUndistortedDirectory(), "captureA", "png");
             if (destination != null) {
                 DirectoryUtil.observe(Paths.get(config.cameraAPreUndistortedDirectory()))
-                    .filter(destination.toPath()::equals).take(1)
+                    .subscribeOn(scheduler)
+                    .observeOn(scheduler)
+                    .filter(destination.getAbsoluteFile().toPath()::equals).take(1)
                     .timeout(1, TimeUnit.MINUTES)
                     .delay(1, TimeUnit.SECONDS)
-                    .observeOn(scheduler)
                     .subscribe(path -> corrector.apply(destination, saveFile));
                 eventPublisher.emit(new CameraCaptureValueA(destination.getAbsolutePath()));
             }
@@ -120,14 +121,15 @@ public void captureUndistortedImageA(final File saveFile) {
      * @param saveFile the output destination
      */
     public void captureUndistortedImageB(final File saveFile) {
-        store.get(config.cameraAName()).map(DistortionCorrector::new).ifPresent(corrector -> {
+        store.get(config.cameraBName()).map(DistortionCorrector::new).ifPresent(corrector -> {
             final File destination = FileUtil.nextName(config.cameraBPreUndistortedDirectory(), "captureB", "png");
             if (destination != null) {
                 DirectoryUtil.observe(Paths.get(config.cameraBPreUndistortedDirectory()))
-                    .filter(destination.toPath()::equals).take(1)
+                    .subscribeOn(scheduler)
+                    .observeOn(scheduler)
+                    .filter(destination.getAbsoluteFile().toPath()::equals).take(1)
                     .timeout(1, TimeUnit.MINUTES)
                     .delay(1, TimeUnit.SECONDS)
-                    .observeOn(scheduler)
                     .subscribe(path -> corrector.apply(destination, saveFile));
                 eventPublisher.emit(new CameraCaptureValueB(destination.getAbsolutePath()));
             }
diff --git a/src/main/java/com/easternedgerobotics/rov/video/DistortionCorrector.java b/src/main/java/com/easternedgerobotics/rov/video/DistortionCorrector.java
index 59d6194..84d1d8a 100644
--- a/src/main/java/com/easternedgerobotics/rov/video/DistortionCorrector.java
+++ b/src/main/java/com/easternedgerobotics/rov/video/DistortionCorrector.java
@@ -6,13 +6,8 @@
 import org.bytedeco.javacpp.opencv_core.Mat;
 import org.bytedeco.javacpp.opencv_imgcodecs;
 import org.bytedeco.javacpp.opencv_imgproc;
-import org.pmw.tinylog.Logger;
 
 import java.io.File;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Path;
-import java.nio.file.StandardCopyOption;
 
 public final class DistortionCorrector {
     /**
@@ -56,16 +51,10 @@ public File apply(
         final File input,
         final File output
     ) {
-        try {
-            final Mat image = opencv_imgcodecs.imread(input.getAbsolutePath());
-            final Mat undistorted = new Mat();
-            opencv_imgproc.remap(image, undistorted, map1, map2, opencv_imgproc.INTER_LINEAR);
-            final Path temp = File.createTempFile("TcpFileReceiver", ".tmp").toPath();
-            opencv_imgcodecs.imwrite(temp.toAbsolutePath().toString(), undistorted);
-            Files.copy(temp, output.toPath(), StandardCopyOption.REPLACE_EXISTING);
-        } catch (final IOException e) {
-            Logger.error(e);
-        }
+        final Mat image = opencv_imgcodecs.imread(input.getAbsolutePath());
+        final Mat undistorted = new Mat();
+        opencv_imgproc.remap(image, undistorted, map1, map2, opencv_imgproc.INTER_LINEAR);
+        opencv_imgcodecs.imwrite(output.getAbsolutePath(), undistorted);
         return output;
     }
 }
diff --git a/src/main/kotlin/com/easternedgerobotics/rov/value/AxisValue.kt b/src/main/kotlin/com/easternedgerobotics/rov/value/AxisValue.kt
new file mode 100644
index 0000000..8ae4673
--- /dev/null
+++ b/src/main/kotlin/com/easternedgerobotics/rov/value/AxisValue.kt
@@ -0,0 +1,3 @@
+package com.easternedgerobotics.rov.value
+
+data class AxisValue(val origin : PointValue, val x : PointValue, val y : PointValue, val z : PointValue)
diff --git a/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCalibrationValue.kt b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCalibrationValue.kt
index 5534f8c..b2fcd85 100644
--- a/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCalibrationValue.kt
+++ b/src/main/kotlin/com/easternedgerobotics/rov/value/CameraCalibrationValue.kt
@@ -32,9 +32,13 @@ data class CameraCalibrationValue(
         val matrix = opencv_core.Mat(3, 3, opencv_core.CV_64F)
         val indexer = matrix.createIndexer<DoubleIndexer>()
         indexer.put(0, 0, fx)
-        indexer.put(1, 1, fy)
+        indexer.put(0, 1, 0.0)
         indexer.put(0, 2, cx)
+        indexer.put(1, 0, 0.0)
+        indexer.put(1, 1, fy)
         indexer.put(1, 2, cy)
+        indexer.put(2, 0, 0.0)
+        indexer.put(2, 1, 0.0)
         indexer.put(2, 2, 1.0)
         return matrix
     }
diff --git a/src/main/kotlin/com/easternedgerobotics/rov/value/PointValue.kt b/src/main/kotlin/com/easternedgerobotics/rov/value/PointValue.kt
new file mode 100644
index 0000000..b1eac9e
--- /dev/null
+++ b/src/main/kotlin/com/easternedgerobotics/rov/value/PointValue.kt
@@ -0,0 +1,3 @@
+package com.easternedgerobotics.rov.value
+
+data class PointValue(val x : Double, val y : Double)