/**
   * Computes the mean/average of two points.
   *
   * @param a (input) Point A
   * @param b (input) Point B
   * @param mean (output) average of 'a' and 'b'
   */
  public static Point2D_F32 mean(Point2D_F32 a, Point2D_F32 b, Point2D_F32 mean) {
    if (mean == null) mean = new Point2D_F32();

    mean.x = (a.x + b.x) / 2.0f;
    mean.y = (a.y + b.y) / 2.0f;

    return mean;
  }
  public static List<Point2D_F32> copy(List<Point2D_F32> pts) {
    List<Point2D_F32> ret = new ArrayList<Point2D_F32>();

    for (Point2D_F32 p : pts) {
      ret.add(p.copy());
    }

    return ret;
  }
  public static List<Point2D_F32> random(float min, float max, int num, Random rand) {
    List<Point2D_F32> ret = new ArrayList<Point2D_F32>();

    float d = max - min;

    for (int i = 0; i < num; i++) {
      Point2D_F32 p = new Point2D_F32();
      p.x = rand.nextFloat() * d + min;
      p.y = rand.nextFloat() * d + min;

      ret.add(p);
    }

    return ret;
  }
  /**
   * Finds the point which has the mean location of all the points in the list. This is also known
   * as the centroid.
   *
   * @param list List of points
   * @param mean Storage for mean point. If null then a new instance will be declared
   * @return The found mean
   */
  public static Point2D_F32 mean(List<Point2D_F32> list, Point2D_F32 mean) {
    if (mean == null) mean = new Point2D_F32();

    float x = 0;
    float y = 0;

    for (Point2D_F32 p : list) {
      x += p.getX();
      y += p.getY();
    }

    x /= list.size();
    y /= list.size();

    mean.set(x, y);
    return mean;
  }
  public static void drawNumbers(
      Graphics2D g2,
      CalibrationObservation foundTarget,
      PointTransform_F32 transform,
      double scale) {

    Font regular = new Font("Serif", Font.PLAIN, 16);
    g2.setFont(regular);

    Point2D_F32 adj = new Point2D_F32();

    AffineTransform origTran = g2.getTransform();
    for (int i = 0; i < foundTarget.size(); i++) {
      Point2D_F64 p = foundTarget.observations.get(i);
      int gridIndex = foundTarget.indexes.get(i);

      if (transform != null) {
        transform.compute((float) p.x, (float) p.y, adj);
      } else {
        adj.set((float) p.x, (float) p.y);
      }

      String text = String.format("%2d", gridIndex);

      int x = (int) (adj.x * scale);
      int y = (int) (adj.y * scale);

      g2.setColor(Color.BLACK);
      g2.drawString(text, x - 1, y);
      g2.drawString(text, x + 1, y);
      g2.drawString(text, x, y - 1);
      g2.drawString(text, x, y + 1);
      g2.setTransform(origTran);
      g2.setColor(Color.GREEN);
      g2.drawString(text, x, y);
    }
  }
  private void drawFeatures(Graphics2D g2, double scale) {

    g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
    g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

    CalibrationObservation set = features.get(selectedImage);

    Point2D_F32 adj = new Point2D_F32();

    if (showPoints) {

      g2.setColor(Color.BLACK);
      g2.setStroke(new BasicStroke(3));
      for (Point2D_F64 p : set.observations) {
        if (showUndistorted) {
          remove_p_to_p.compute((float) p.x, (float) p.y, adj);
        } else {
          adj.set((float) p.x, (float) p.y);
        }
        VisualizeFeatures.drawCross(g2, adj.x * scale, adj.y * scale, 4);
      }
      g2.setStroke(new BasicStroke(1));
      g2.setColor(Color.RED);
      for (Point2D_F64 p : set.observations) {
        if (showUndistorted) {
          remove_p_to_p.compute((float) p.x, (float) p.y, adj);
        } else {
          adj.set((float) p.x, (float) p.y);
        }
        VisualizeFeatures.drawCross(g2, adj.x * scale, adj.y * scale, 4);
      }
    }

    if (showAll) {
      for (CalibrationObservation l : features) {
        for (Point2D_F64 p : l.observations) {
          if (showUndistorted) {
            remove_p_to_p.compute((float) p.x, (float) p.y, adj);
          } else {
            adj.set((float) p.x, (float) p.y);
          }
          VisualizeFeatures.drawPoint(g2, adj.x * scale, adj.y * scale, 2, Color.BLUE, false);
        }
      }
    }

    if (showNumbers) {
      if (showUndistorted) drawNumbers(g2, set, remove_p_to_p, scale);
      else drawNumbers(g2, set, null, scale);
    }

    if (showErrors && results != null && results.size() > selectedImage) {
      ImageResults result = results.get(selectedImage);

      Stroke before = g2.getStroke();
      g2.setStroke(new BasicStroke(4));
      g2.setColor(Color.BLACK);
      for (int i = 0; i < set.size(); i++) {
        Point2D_F64 p = set.observations.get(i);

        if (showUndistorted) {
          remove_p_to_p.compute((float) p.x, (float) p.y, adj);
        } else {
          adj.set((float) p.x, (float) p.y);
        }

        double r = errorScale * result.pointError[i];
        if (r < 1) continue;

        VisualizeFeatures.drawCircle(g2, adj.x * scale, adj.y * scale, r);
      }

      g2.setStroke(before);
      g2.setColor(Color.ORANGE);
      for (int i = 0; i < set.size(); i++) {
        Point2D_F64 p = set.observations.get(i);

        if (showUndistorted) {
          remove_p_to_p.compute((float) p.x, (float) p.y, adj);
        } else {
          adj.set((float) p.x, (float) p.y);
        }

        double r = errorScale * result.pointError[i];
        if (r < 1) continue;

        VisualizeFeatures.drawCircle(g2, adj.x * scale, adj.y * scale, r);
      }
    }
  }
 public static void noiseNormal(List<Point2D_F32> pts, float sigma, Random rand) {
   for (Point2D_F32 p : pts) {
     p.x += (float) rand.nextGaussian() * sigma;
     p.y += (float) rand.nextGaussian() * sigma;
   }
 }