/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

// TODO(oalexan1): Clean these headers.
// TODO(oalexan1): Move some things to utils

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/problem.h>
#include <ceres/solver.h>
#include <ceres/cost_function.h>
#include <ceres/loss_function.h>
#include <ceres/dynamic_numeric_diff_cost_function.h>
#include <ceres/numeric_diff_cost_function.h>
#include <ceres/autodiff_cost_function.h>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/utility.hpp>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <pcl_ros/point_cloud.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <Eigen/Geometry>
#include <Eigen/Core>

#include <ff_common/utils.h>
#include <sparse_mapping/sparse_map.h>
#include <sparse_mapping/reprojection.h>
#include <config_reader/config_reader.h>
#include <msg_conversions/msg_conversions.h>

#include <dense_map_ros_utils.h>
#include <dense_map_utils.h>

#include <string>
#include <map>
#include <iostream>
#include <fstream>

DEFINE_string(cam1_name, "", "The name of the first camera. Must be 'nav_cam' or 'sci_cam'.");

DEFINE_string(cam2_name, "haz_cam", "The name of the second camera. Must be haz_cam.");

DEFINE_string(cam1_dir, "", "The directory having the corners extracted from cam1 images using Kalibr.");

DEFINE_string(cam2_dir, "", "The directory having the corners extracted from cam2 images using Kalibr.");

DEFINE_string(bag, "", "The ros bag having the calibration data.");

DEFINE_string(haz_cam_points_topic, "/hw/depth_haz/points", "The depth point cloud topic in the bag file.");

DEFINE_double(start, 0.0, "The starting time in seconds from which to process the bag.");

DEFINE_double(duration, -1.0, "Specify how many seconds from the bag to process (default: process the whole bag).");

DEFINE_bool(update_cam1, false, "Update the intrinsics of cam1.");

DEFINE_bool(update_cam2, false, "Update the intrinsics of cam2.");

DEFINE_bool(update_depth_to_image_transform, false,
            "Update the transform to apply to scale correctly the depth camera clouds.");

DEFINE_bool(update_extrinsics, false, "Update the transform from cam2 to cam1.");

DEFINE_string(timestamp_offset_sampling, "",
              "If specified, as 'beg end num', create num samples between beg and end, "
              "and determine the timestamp offset between these cameras as the sample "
              "minimizing the extrinsics error among cam1 and cam2.");

DEFINE_string(cam1_intrinsics_to_float, "",
              "Refine 0 or more of the following intrinsics for cam1: focal_length, "
              "optical_center, distortion. Specify as a quoted list. "
              "For example: 'focal_length optical_center'.");

DEFINE_string(cam2_intrinsics_to_float, "",
              "Refine 0 or more of the following intrinsics for cam2: focal_length, "
              "optical_center, distortion. Specify as a quoted list. "
              "For example: 'focal_length optical_center'.");

DEFINE_int32(num_cam1_focal_lengths, 1,
             "If set to 2, use separate focal lengths along image rows and columns for cam1. Else use the same one.");

DEFINE_int32(num_cam2_focal_lengths, 1,
             "If set to 2, use separate focal lengths along image rows and columns for cam2. Else use the same one.");

DEFINE_int32(num_cam1_iterations, 10000, "How many solver iterations to perform to solve for cam1 intrinsics.");

DEFINE_int32(num_cam2_iterations, 10000, "How many solver iterations to perform to solve for cam2 intrinsics.");

DEFINE_int32(num_extrinsics_iterations, 10000, "How many solver iterations to perform to solve for extrinsics.");

DEFINE_double(robust_threshold, 4.0,
              "Pixel errors much larger than this will be exponentially attenuated "
              "to affect less the cost function. This should not be too low, "
              "as if the initial guess is off, initial pixel errors can be "
              "rather large.");

DEFINE_double(parameter_tolerance, 1e-12, "Stop when the optimization variables change by less than this.");

DEFINE_int32(calib_num_ransac_iterations, 1000,
             "Use in this many RANSAC iterations to find camera poses based on "
             "calibration target measurements.");

DEFINE_int32(calib_ransac_inlier_tolerance, 10, "Use this inlier tolerance (in pixels) to find camera poses.");

DEFINE_double(max_interp_dist, 4.0,
              "If two consecutive camera poses have timestamps that differ more than this,"
              "do not interpolate between them.");

DEFINE_double(max_haz_cam_image_to_depth_timestamp_diff, 0.2,
              "Use depth haz cam clouds that are within this distance in "
              "time from the nearest haz cam intensity image.");

DEFINE_string(output_dir, "",
              "If provided, write here the residuals differences between pixel target "
              "corners and projection into cameras of measured target points) before "
              "and after optimization.");

namespace dense_map {
ceres::LossFunction* GetLossFunction(std::string cost_fun, double th) {
  // Convert to lower-case
  std::transform(cost_fun.begin(), cost_fun.end(), cost_fun.begin(), ::tolower);

  ceres::LossFunction* loss_function = NULL;
  if (cost_fun == "l2")
    loss_function = NULL;
  else if (cost_fun == "huber")
    loss_function = new ceres::HuberLoss(th);
  else if (cost_fun == "cauchy")
    loss_function = new ceres::CauchyLoss(th);
  else if (cost_fun == "l1")
    loss_function = new ceres::SoftLOneLoss(th);
  else
    LOG(FATAL) << "Unknown cost function: " + cost_fun;

  return loss_function;
}

// An error function for finding the best intrinsics of a camera
// by taking n pictures of a calibration target and computing
// the reprojection error for given intrinsics and camera orientation
// relative to the target.
struct IntrinsicsError {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  IntrinsicsError(Eigen::Vector2d const& pix, Eigen::Vector3d const& xyz, std::vector<int> const& block_sizes,
                  boost::shared_ptr<camera::CameraParameters> orig_cam_params)
      : m_pix(pix), m_xyz(xyz), m_block_sizes(block_sizes), m_orig_cam_params(orig_cam_params) {
    // Sanity check. Note that we can have one or two focal lengths.
    if (m_block_sizes.size() != 4 || (m_block_sizes[0] != 1 && m_block_sizes[0] != 2) || m_block_sizes[1] != 2 ||
        m_block_sizes[2] <= 0 || m_block_sizes[3] != NUM_RIGID_PARAMS) {
      LOG(FATAL) << "IntrinsicsError: The block sizes were not set up properly.\n";
    }
  }

  // Call to work with ceres::DynamicNumericDiffCostFunction.
  // Takes array of arrays as parameters.
  bool operator()(double const* const* parameters, double* residuals) const {
    // Populate the intrinsics

    Eigen::Vector2d focal_vector;
    if (m_block_sizes[0] == 1)
      focal_vector = Eigen::Vector2d(parameters[0][0], parameters[0][0]);  // 1 focal length
    else
      focal_vector = Eigen::Vector2d(parameters[0][0], parameters[0][1]);  // 2 focal lengths

    Eigen::Vector2d optical_center(parameters[1][0], parameters[1][1]);
    Eigen::VectorXd distortion(m_block_sizes[2]);
    for (int i = 0; i < m_block_sizes[2]; i++) distortion[i] = parameters[2][i];

    // Make a deep copy which we will modify
    camera::CameraParameters cam_params = *m_orig_cam_params;

    // Set the current values of the parameters we optimize
    cam_params.SetFocalLength(focal_vector);
    cam_params.SetOpticalOffset(optical_center);
    cam_params.SetDistortion(distortion);

    // The current transform from the world to the camera
    Eigen::Affine3d world_to_cam;
    array_to_rigid_transform(world_to_cam, parameters[3]);

    // Project the measured calibration target corner into the camera
    Eigen::Vector3d cX = world_to_cam * m_xyz;
    Eigen::Vector2d undist_pix = cam_params.GetFocalVector().cwiseProduct(cX.hnormalized());
    Eigen::Vector2d dist_pix;
    cam_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_pix, &dist_pix);

    // Compute the residuals
    residuals[0] = dist_pix[0] - m_pix[0];
    residuals[1] = dist_pix[1] - m_pix[1];

    return true;
  }

  // Factory to hide the construction of the CostFunction object from the client code.
  static ceres::CostFunction* Create(Eigen::Vector2d const& pix, Eigen::Vector3d const& xyz,
                                     std::vector<int> const& block_sizes,
                                     boost::shared_ptr<camera::CameraParameters> orig_cam_params) {
    ceres::DynamicNumericDiffCostFunction<IntrinsicsError>* cost_function =
      new ceres::DynamicNumericDiffCostFunction<IntrinsicsError>(
        new IntrinsicsError(pix, xyz, block_sizes, orig_cam_params));

    // The residual size is always the same.
    cost_function->SetNumResiduals(NUM_RESIDUALS);

    // The camera wrapper knows all of the block sizes to add.
    for (size_t i = 0; i < block_sizes.size(); i++) {
      cost_function->AddParameterBlock(block_sizes[i]);
    }
    return cost_function;
  }

 private:
  Eigen::Vector2d m_pix;  // The pixel observation
  Eigen::Vector3d m_xyz;  // The measured position
  std::vector<int> m_block_sizes;
  boost::shared_ptr<camera::CameraParameters> m_orig_cam_params;  // the camera before optimization
};                                                                // End class IntrinsicsError

// An error function for projecting 3D measured points to measured pixels,
// and finding the optimal intrinsics for minimizing the projection error.
// This is used to calibrate the haz_cam intrinsics, to make its depth
// measurements consistent with its intensity image.
struct IntrinsicsDepthError {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  IntrinsicsDepthError(Eigen::Vector2d const& pix, Eigen::Vector3d const& xyz, std::vector<int> const& block_sizes,
                       boost::shared_ptr<camera::CameraParameters> orig_cam_params)
      : m_pix(pix), m_xyz(xyz), m_block_sizes(block_sizes), m_orig_cam_params(orig_cam_params) {
    // Sanity check. Note that we can have one or two focal lengths.
    if (m_block_sizes.size() != 3 || (m_block_sizes[0] != 1 && m_block_sizes[0] != 2) || m_block_sizes[1] != 2 ||
        m_block_sizes[2] <= 0) {
      LOG(FATAL) << "IntrinsicsError: The block sizes were not set up properly.\n";
    }
  }

  // Call to work with ceres::DynamicNumericDiffCostFunction.
  // Takes array of arrays as parameters.
  bool operator()(double const* const* parameters, double* residuals) const {
    // Populate the intrinsics

    Eigen::Vector2d focal_vector;
    if (m_block_sizes[0] == 1)
      focal_vector = Eigen::Vector2d(parameters[0][0], parameters[0][0]);  // 1 focal length
    else
      focal_vector = Eigen::Vector2d(parameters[0][0], parameters[0][1]);  // 2 focal lengths

    Eigen::Vector2d optical_center(parameters[1][0], parameters[1][1]);
    Eigen::VectorXd distortion(m_block_sizes[2]);
    for (int i = 0; i < m_block_sizes[2]; i++) distortion[i] = parameters[2][i];

    // Make a deep copy which we will modify
    camera::CameraParameters cam_params = *m_orig_cam_params;

    // Set the current values of the parameters we optimize
    cam_params.SetFocalLength(focal_vector);
    cam_params.SetOpticalOffset(optical_center);
    cam_params.SetDistortion(distortion);

    //     std::cout << "focal vector is " << focal_vector.transpose() << std::endl;
    //     std::cout << "optical center is " << optical_center.transpose() << std::endl;
    //     std::cout << "distortion is " << distortion.transpose() << std::endl;

    // Project the xyz point into the camera
    // Note that undist_pix is relative to the center of the image
    // but dist_pix is positive (relative to (0, 0) image corner)
    Eigen::Vector2d undist_pix = cam_params.GetFocalVector().cwiseProduct(m_xyz.hnormalized());
    Eigen::Vector2d dist_pix;
    cam_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_pix, &dist_pix);

    //     std::cout << "--dist 0 " << dist_pix[0] << ' ' << m_pix[0] << std::endl;
    //     std::cout << "--dist 1 " << dist_pix[1] << ' ' << m_pix[1] << std::endl;

    residuals[0] = dist_pix[0] - m_pix[0];
    residuals[1] = dist_pix[1] - m_pix[1];

    //     std::cout << "--residual is " << residuals[0] << ' ' << residuals[1] << std::endl;

    return true;
  }

  // Factory to hide the construction of the CostFunction object from the client code.
  static ceres::CostFunction* Create(Eigen::Vector2d const& pix, Eigen::Vector3d const& xyz,
                                     std::vector<int> const& block_sizes,
                                     boost::shared_ptr<camera::CameraParameters> orig_cam_params) {
    ceres::DynamicNumericDiffCostFunction<IntrinsicsDepthError>* cost_function =
      new ceres::DynamicNumericDiffCostFunction<IntrinsicsDepthError>(
        new IntrinsicsDepthError(pix, xyz, block_sizes, orig_cam_params));

    // The residual size is always the same.
    cost_function->SetNumResiduals(NUM_RESIDUALS);

    // The camera wrapper knows all of the block sizes to add.
    for (size_t i = 0; i < block_sizes.size(); i++) {
      cost_function->AddParameterBlock(block_sizes[i]);
    }
    return cost_function;
  }

 private:
  Eigen::Vector2d m_pix;  // The pixel observation
  Eigen::Vector3d m_xyz;  // The measured position
  std::vector<int> m_block_sizes;
  boost::shared_ptr<camera::CameraParameters> m_orig_cam_params;  // the camera before optimization
};                                                                // End class IntrinsicsDepthError

// A Ceres cost function solving for extrinsics, so
// cam2_to_cam1_transform. Model the fact that cam1
// and cam2 were acquired at different times.
struct ExtrinsicsError {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ExtrinsicsError(Eigen::Vector2d const& target_corner_pixel, Eigen::Vector2d const& target_corner_meas,
                  Eigen::Affine3d const& interp_world_to_cam1, std::vector<int> const& block_sizes,
                  boost::shared_ptr<camera::CameraParameters> cam2_params)
      : m_target_corner_pixel(target_corner_pixel),
        m_target_corner_meas(target_corner_meas),
        m_interp_world_to_cam1(interp_world_to_cam1),
        m_block_sizes(block_sizes),
        m_cam2_params(cam2_params) {}

  // Call to work with ceres::DynamicNumericDiffCostFunction.
  bool operator()(double const* const* parameters, double* residuals) const {
    try {
      if (m_block_sizes.size() != 1 || m_block_sizes[0] != NUM_RIGID_PARAMS)
        LOG(FATAL) << "ExtrinsicsError: The block sizes were not set up properly.\n";

      Eigen::Affine3d cam2_to_cam1_transform;
      array_to_rigid_transform(cam2_to_cam1_transform, parameters[0]);

      Eigen::Affine3d world_to_cam2 = cam2_to_cam1_transform.inverse() * m_interp_world_to_cam1;

      Eigen::Vector3d X;
      X[0] = m_target_corner_meas[0];
      X[1] = m_target_corner_meas[1];
      X[2] = 0;

      Eigen::Vector3d cX0 = world_to_cam2 * X;
      Eigen::Vector2d undist_pix0 = m_cam2_params->GetFocalVector().cwiseProduct((cX0).hnormalized());
      Eigen::Vector2d dist_pix0;
      m_cam2_params->Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_pix0, &dist_pix0);

      residuals[0] = dist_pix0[0] - m_target_corner_pixel[0];
      residuals[1] = dist_pix0[1] - m_target_corner_pixel[1];

      if (std::isnan(residuals[0]) || std::isnan(residuals[1])) {
        residuals[0] = 0.0;
        residuals[1] = 0.0;
      }
    } catch (std::exception const& e) {
      // If anything goes wrong just set the residuals to 0
      residuals[0] = 0.0;
      residuals[1] = 0.0;
    }

    return true;
  }

  // Factory to hide the construction of the CostFunction object from the client code.
  static ceres::CostFunction* Create(Eigen::Vector2d const& target_corner_pixel,
                                     Eigen::Vector2d const& target_corner_meas,
                                     Eigen::Affine3d const& interp_world_to_cam1, std::vector<int> const& block_sizes,
                                     boost::shared_ptr<camera::CameraParameters> cam2_params) {
    ceres::DynamicNumericDiffCostFunction<ExtrinsicsError>* cost_function =
      new ceres::DynamicNumericDiffCostFunction<ExtrinsicsError>(
        new ExtrinsicsError(target_corner_pixel, target_corner_meas, interp_world_to_cam1, block_sizes, cam2_params));

    // The residual size is always the same.
    cost_function->SetNumResiduals(NUM_RESIDUALS);

    // The camera wrapper knows all of the block sizes to add.
    for (size_t i = 0; i < block_sizes.size(); i++) {
      cost_function->AddParameterBlock(block_sizes[i]);
    }
    return cost_function;
  }

 private:
  Eigen::Vector2d m_target_corner_pixel;
  Eigen::Vector2d m_target_corner_meas;
  Eigen::Affine3d m_interp_world_to_cam1;
  std::vector<int> m_block_sizes;
  boost::shared_ptr<camera::CameraParameters> m_cam2_params;
};  // End class ExtrinsicsError

void read_measurements(std::string const& filename, std::vector<std::vector<double>>& meas) {
  // std::cout << "Reading: " << filename << std::endl;
  std::ifstream cr(filename.c_str());
  std::string line;
  meas.clear();
  while (std::getline(cr, line)) {
    double val;
    std::vector<double> vals;
    std::istringstream is(line);
    while (is >> val) vals.push_back(val);
    if (vals.size() != 5) LOG(FATAL) << "Each line of " << filename << " must have 5 entries.";
    meas.push_back(vals);
  }
}

// Read all calibration target corners detected with Kalibr
void read_target_corners(  // Inputs
  double start, double duration, std::string const& data_dir,
  // Outputs
  std::map<double, std::vector<std::vector<double>>>& target_corners) {
  // Wipe the inputs
  target_corners.clear();
  double beg_timestamp = -1.0;

  std::vector<std::string> corners_files;
  ff_common::ListFiles(data_dir, "txt", &corners_files);
  for (size_t it = 0; it < corners_files.size(); it++) {
    std::string corners_file = corners_files[it];

    // Initialize the beg timestamp
    double timestamp = dense_map::fileNameToTimestamp(corners_file);

    if (timestamp <= 0) {
      std::cout << "Ignoring file: " << corners_file << std::endl;
      continue;  // likely the wrong kind of file
    }

    if (beg_timestamp < 0.0) beg_timestamp = timestamp;

    if (timestamp - beg_timestamp < start) continue;  // did not yet reach the desired starting time

    if (duration > 0 && timestamp - beg_timestamp > start + duration) break;  // past the desired ending time

    std::vector<std::vector<double>> corners;
    read_measurements(corners_file, corners);
    if (corners.empty()) continue;

    target_corners[timestamp] = corners;
  }
}

// Find the depth cam measurement at each target point (in that
// camera's coordinate system). The target points come from the
// intensity image, while the depth measurements from the depth cloud,
// which have slightly different timestamps. So some bracketing is
// required.
void read_depth_cam_meas(  // Inputs
  std::string const& bag_file, std::string const& haz_cam_points_topic,
  std::map<double, std::vector<std::vector<double>>> const& cam2_target_corners,
  // Outputs
  std::map<double, std::vector<Eigen::Vector3d>>& target_corners_depth) {
  // Wipe the outputs
  target_corners_depth.clear();

  // It is convenient to have the timestamps for the intensity in a vector
  std::vector<double> cam2_timestamps;
  for (auto it = cam2_target_corners.begin(); it != cam2_target_corners.end(); it++) {
    // They better be in increasing order
    if (!cam2_timestamps.empty() && cam2_timestamps.back() >= it->first)
      LOG(FATAL) << "Expecting timestamps to increase.";

    cam2_timestamps.push_back(it->first);
  }

  // Iterate over the depth cam clouds
  rosbag::Bag bag;
  bag.open(bag_file, rosbag::bagmode::Read);

  std::vector<std::string> topics;
  topics.push_back(haz_cam_points_topic);
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  // Keep track of the index of the haz cam image whose timestamp
  // is less but close to the timestamp of the haz cam cloud.
  size_t left_haz_cam_image_index = 0;

  // Process each haz cam cloud by bracketing it between haz cam
  // image timestamps and finding the image that is closest in time.
  for (rosbag::MessageInstance const m : view) {
    sensor_msgs::PointCloud2::ConstPtr pc_msg = m.instantiate<sensor_msgs::PointCloud2>();
    if (!pc_msg) continue;
    double cloud_time = pc_msg->header.stamp.toSec();

    if (cloud_time < cam2_timestamps[left_haz_cam_image_index]) {
      // We are too early in the bag, not yet at the point where we have
      // data to process
      continue;
    }

    if (cloud_time >= cam2_timestamps.back()) {
      // We passed the last timestamp for which we have data to process
      break;
    }

    // Bracket the cloud time between two depth cam image timestamps
    for (size_t cam2_cid = left_haz_cam_image_index; cam2_cid + 1 < cam2_timestamps.size(); cam2_cid++) {
      double left_haz_cam_image_time = cam2_timestamps[cam2_cid + 0];
      double right_haz_cam_image_time = cam2_timestamps[cam2_cid + 1];

      // Save for the future this index
      left_haz_cam_image_index = cam2_cid;

      if (cloud_time >= right_haz_cam_image_time) {
        // Continue advancing in time with cam2_cid.
        continue;
      }

      // So now left_haz_cam_image_time <= cloud_time < right_haz_cam_image_time
      if (cloud_time < left_haz_cam_image_time || cloud_time >= right_haz_cam_image_time)
        LOG(FATAL) << "Book-keeping failure.";

      size_t image_cid = 0;
      if (std::abs(left_haz_cam_image_time - cloud_time) <= std::abs(right_haz_cam_image_time - cloud_time)) {
        image_cid = cam2_cid;
      } else {
        image_cid = cam2_cid + 1;
      }

      // The haz cam image should be normally quite close to the haz
      // cam cloud. Else the results won't be accurate.
      double diff_time = std::abs(cam2_timestamps[image_cid] - cloud_time);
      if (diff_time > FLAGS_max_haz_cam_image_to_depth_timestamp_diff) {
        std::cout << "Found a gap of " << diff_time << " seconds between haz cam intensity and "
                  << "depth timestamps. This is too big. Skipping this frame.\n";
        break;
      }

      pcl::PointCloud<pcl::PointXYZ> pc;
      pcl::fromROSMsg(*pc_msg, pc);
      if (static_cast<int>(pc.points.size()) != static_cast<int>(pc_msg->width * pc_msg->height))
        LOG(FATAL) << "Extracted point cloud size does not agree with original size.";

      double best_image_timestamp = cam2_timestamps[image_cid];
      auto it = cam2_target_corners.find(best_image_timestamp);
      if (it == cam2_target_corners.end()) LOG(FATAL) << "Cannot locate target corners at desired timestamp.";

      // Will push here one depth measurement for each target corners.
      // Invalid ones will be (0, 0, 0).
      target_corners_depth[best_image_timestamp].clear();

      std::vector<std::vector<double>> const& target_corners = it->second;
      for (size_t pt_iter = 0; pt_iter < target_corners.size(); pt_iter++) {
        Eigen::Vector2d target_corner_pixel;
        // The value  target_corners[pt_iter][0] is the corner id
        // Then come the world measurements, and pixel measurements
        target_corner_pixel[0] = target_corners[pt_iter][3];  // projection into camera x
        target_corner_pixel[1] = target_corners[pt_iter][4];  // projection into camera y

        // Round to find the index in the depth cloud
        size_t col = round(target_corner_pixel[0]);
        size_t row = round(target_corner_pixel[1]);

        if (col < 0 || row < 0 || col > pc_msg->width || row > pc_msg->height) {
          LOG(FATAL) << "Invalid target corner pixel: " << target_corner_pixel.transpose() << std::endl;
        }

        Eigen::Vector3d depth_pt;
        depth_pt << 0.0, 0.0, 0.0;

        // Maybe after rounding we got out of range, so have to check
        if (col < pc_msg->width && row < pc_msg->height) {
          size_t count = row * pc_msg->width + col;
          depth_pt[0] = pc.points[count].x;
          depth_pt[1] = pc.points[count].y;
          depth_pt[2] = pc.points[count].z;
        }

        target_corners_depth[best_image_timestamp].push_back(depth_pt);
      }

      // Bracketing was successful, and the current cloud was processed
      break;
    }
  }  // End iterating over the bag
}

// Create the vector timestamp offset samples. If none are specified
// on the command line, use only the one specified in the config file.
void parse_timestamp_offset_sampling(double cam1_to_cam2_timestamp_offset, std::string const& timestamp_offset_sampling,
                                     std::vector<double>& samples) {
  std::istringstream is(timestamp_offset_sampling);

  if (timestamp_offset_sampling == "") {
    samples.push_back(cam1_to_cam2_timestamp_offset);
  } else {
    std::string val;
    double beg, end;
    int num;

    if (!(is >> beg >> end >> num)) LOG(FATAL) << "Could not parse " << timestamp_offset_sampling << std::endl;

    if (beg > end || num <= 0) LOG(FATAL) << "The timestamp offset sampling parameters are invalid.";

    samples.clear();
    double delta = 0.0;
    if (num > 1) delta = (end - beg) / (num - 1);
    for (int it = 0; it < num - 1; it++) samples.push_back(beg + it * delta);
    samples.push_back(end);  // add the last one while avoiding any rounding error
  }
}

// Find the camera poses for the images having the calibration target
// (the calibration target is assumed to be fixed).
void find_world_to_cam_trans(  // Inputs
  std::string const& cam_name, boost::shared_ptr<camera::CameraParameters> cam_params,
  std::map<double, std::vector<std::vector<double>>> const& target_corners,
  // Outputs
  std::vector<double>& timestamps, std::vector<std::vector<Eigen::Vector2d>>& observations,
  std::vector<std::vector<Eigen::Vector3d>>& landmarks, std::vector<Eigen::Affine3d>& world_to_cam_vec) {
  // Wipe the inputs
  timestamps.clear();
  observations.clear();
  landmarks.clear();
  world_to_cam_vec.clear();

  for (auto it = target_corners.begin(); it != target_corners.end(); it++) {
    double timestamp = it->first;
    std::vector<std::vector<double>> const& corners = it->second;

    // Find the camera pose
    std::vector<Eigen::Vector2d> observations_per_image;
    std::vector<Eigen::Vector2d> undist_observations_per_image;
    std::vector<Eigen::Vector3d> landmarks_per_image;
    for (size_t pt_iter = 0; pt_iter < corners.size(); pt_iter++) {
      // The value  corners[pt_iter][0] is the corner id

      // Record the known measurements of the calibration target corners in meters
      landmarks_per_image.push_back(Eigen::Vector3d(corners[pt_iter][1], corners[pt_iter][2], 0.0));

      // Record the pixel position of the calibration corners as detected in the image
      Eigen::Vector2d target_corner_pixel;
      target_corner_pixel[0] = corners[pt_iter][3];  // projection into camera x in pixels
      target_corner_pixel[1] = corners[pt_iter][4];  // projection into camera y in pixels
      observations_per_image.push_back(target_corner_pixel);

      Eigen::Vector2d undist_observation;
      cam_params->Convert<camera::DISTORTED, camera::UNDISTORTED_C>(target_corner_pixel, &undist_observation);
      undist_observations_per_image.push_back(undist_observation);
    }

    Eigen::Affine3d world_to_cam;
    camera::CameraModel cam_model(world_to_cam, *cam_params.get());
    int ret = sparse_mapping::RansacEstimateCamera(landmarks_per_image, undist_observations_per_image,
                                                   FLAGS_calib_num_ransac_iterations,
                                                   FLAGS_calib_ransac_inlier_tolerance, &cam_model);
    if (ret != 0) continue;  // failed to estimate the camera

    world_to_cam = cam_model.GetTransform();
    timestamps.push_back(timestamp);
    observations.push_back(observations_per_image);
    landmarks.push_back(landmarks_per_image);
    world_to_cam_vec.push_back(world_to_cam);
  }
}

double calc_and_print_residual_stats(std::string const& prefix, std::string const& cam_name,
                                     std::vector<double> const& residuals) {
  std::vector<double> err_norm;
  size_t len = residuals.size() / 2;

  if (len == 0) LOG(FATAL) << "No residuals exist.";

  double res = 0.0;
  for (size_t it = 0; it < len; it++) {
    double norm = Eigen::Vector2d(residuals[2 * it + 0], residuals[2 * it + 1]).norm();
    res += norm;
    err_norm.push_back(norm);
  }
  res /= len;

  std::sort(err_norm.begin(), err_norm.end());
  std::cout << prefix << " min, mean, median and max reprojection error for " << cam_name << ": " << err_norm[0] << ' '
            << res << ' ' << err_norm[err_norm.size() / 2] << ' ' << err_norm.back() << std::endl;

  // Return the median error
  return err_norm[err_norm.size() / 2];
}

void save_residuals(std::string const& prefix, std::string const& cam_name, std::string const& output_dir,
                    std::vector<Eigen::Vector2d> const& pixels, std::vector<double> const& residuals) {
  size_t len = residuals.size() / 2;
  if (len != pixels.size()) {
    LOG(FATAL) << "Must have one residual norm per pixel.";
  }

  std::string res_file = output_dir + "/" + prefix + "-" + cam_name + "-residuals.txt";
  std::cout << "Writing residuals to: " << res_file << std::endl;
  std::ofstream ofs(res_file);
  ofs.precision(17);
  ofs << "# pixel_x, pixel_y, residual_x, residual_y, residual_norm\n";
  for (size_t it = 0; it < len; it++) {
    double norm = Eigen::Vector2d(residuals[2 * it + 0], residuals[2 * it + 1]).norm();
    ofs << pixels[it][0] << ", " << pixels[it][1] << ", " << residuals[2 * it + 0] << ", " << residuals[2 * it + 1]
        << ", " << norm << std::endl;
  }
  ofs.close();
}

// Refine a camera's intrinsics given a set images of the calibration target
// (refine along the way the position of the camera when those images were taken)
void refine_intrinsics(  // Inputs
  std::string const& cam_name, int num_focal_lengths, int num_iterations,
  std::set<std::string> const& cam_intrinsics_to_float, std::string const& output_dir,
  // Outputs
  boost::shared_ptr<camera::CameraParameters> cam_params, std::vector<std::vector<Eigen::Vector2d>>& observations,
  std::vector<std::vector<Eigen::Vector3d>>& landmarks, std::vector<Eigen::Affine3d>& world_to_cam_vec) {
  if (observations.size() != landmarks.size() || observations.size() != world_to_cam_vec.size())
    LOG(FATAL) << "Inconsistent input data for refining intrinsics.";

  // Set up the vector having the camera poses to optimzie
  int num_cams = world_to_cam_vec.size();
  std::vector<double> cameras(num_cams * NUM_RIGID_PARAMS);
  for (int it = 0; it < num_cams; it++) rigid_transform_to_array(world_to_cam_vec[it], &cameras[NUM_RIGID_PARAMS * it]);

  // Create storage space for the intrinsics
  Eigen::Vector2d focal_vector = cam_params->GetFocalVector();
  if (num_focal_lengths == 1) {
    focal_vector[0] = cam_params->GetFocalLength();  // average the two focal lengths
    focal_vector[1] = focal_vector[0];
  }
  std::cout << std::endl;
  std::cout << "Initial focal length for " << cam_name << ": " << focal_vector.transpose() << std::endl;
  Eigen::Vector2d optical_center = cam_params->GetOpticalOffset();
  std::cout << "Initial optical center for " << cam_name << ": " << optical_center.transpose() << std::endl;

  Eigen::VectorXd distortion = cam_params->GetDistortion();
  std::cout << "Initial distortion for " << cam_name << ": " << distortion.transpose() << std::endl;

  // Set up the variable blocks to optimize
  std::vector<int> block_sizes;
  block_sizes.push_back(num_focal_lengths);   // focal length
  block_sizes.push_back(NUM_OPT_CTR_PARAMS);  // optical center
  block_sizes.push_back(distortion.size());   // distortion
  block_sizes.push_back(NUM_RIGID_PARAMS);    // camera pose

  // build the problem
  ceres::Problem problem;
  std::vector<Eigen::Vector2d> pixels;
  for (int cam_it = 0; cam_it < num_cams; cam_it++) {
    for (size_t obs_it = 0; obs_it < observations[cam_it].size(); obs_it++) {
      pixels.push_back(observations[cam_it][obs_it]);
      ceres::CostFunction* cost_function =
        IntrinsicsError::Create(observations[cam_it][obs_it], landmarks[cam_it][obs_it], block_sizes, cam_params);

      ceres::LossFunction* loss_function = GetLossFunction("cauchy", FLAGS_robust_threshold);

      problem.AddResidualBlock(cost_function, loss_function, &focal_vector[0], &optical_center[0], &distortion[0],
                               &cameras[NUM_RIGID_PARAMS * cam_it]);

      // In case it is ever desired to experiment with fixing the cameras
      // problem.SetParameterBlockConstant(&cameras[NUM_RIGID_PARAMS * cam_it]);
    }
  }

  // See which values to float or keep fixed
  if (cam_intrinsics_to_float.find("focal_length") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating focal_length." <<
    // std::endl;
    problem.SetParameterBlockConstant(&focal_vector[0]);
  } else {
    // std::cout << "For " << cam_name << " floating focal_length." <<
    // std::endl;
  }
  if (cam_intrinsics_to_float.find("optical_center") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating optical_center." <<
    // std::endl;
    problem.SetParameterBlockConstant(&optical_center[0]);
  } else {
    // std::cout << "For " << cam_name << " floating optical_center." <<
    // std::endl;
  }
  if (cam_intrinsics_to_float.find("distortion") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating distortion." <<
    // std::endl;
    problem.SetParameterBlockConstant(&distortion[0]);
  } else {
    // std::cout << "For " << cam_name << " floating distortion." << std::endl;
  }

  // Evaluate the residual before optimization
  double total_cost = 0.0;
  std::vector<double> err_norm;
  err_norm.clear();
  std::vector<double> residuals;
  ceres::Problem::EvaluateOptions eval_options;
  eval_options.num_threads = 1;
  eval_options.apply_loss_function = false;  // want raw residuals
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);

  calc_and_print_residual_stats("Initial", cam_name, residuals);
  if (output_dir != "") save_residuals("initial", cam_name, output_dir, pixels, residuals);

  // Solve the problem
  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  options.num_threads = 1;  // The result is more predictable with one thread
  options.max_num_iterations = num_iterations;
  options.minimizer_progress_to_stdout = false;
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = FLAGS_parameter_tolerance;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.FullReport() << "\n";

  // Evaluate the residuals after optimization
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);

  calc_and_print_residual_stats("Final", cam_name, residuals);
  if (output_dir != "") save_residuals("final", cam_name, output_dir, pixels, residuals);

  if (num_focal_lengths == 1) focal_vector[1] = focal_vector[0];  // Only focal_vector[0] was used

  std::cout << "Final focal length for " << cam_name << ": " << focal_vector.transpose() << std::endl;
  std::cout << "Final optical center for " << cam_name << ": " << optical_center.transpose() << std::endl;
  std::cout << "Final distortion for " << cam_name << ": " << distortion.transpose() << std::endl;

  // Copy back the optimized intrinsics
  if (num_focal_lengths == 1)
    cam_params->SetFocalLength(Eigen::Vector2d(focal_vector[0], focal_vector[0]));
  else
    cam_params->SetFocalLength(focal_vector);
  cam_params->SetOpticalOffset(optical_center);
  cam_params->SetDistortion(distortion);

  // Copy back the optimized cameras
  for (int it = 0; it < num_cams; it++) array_to_rigid_transform(world_to_cam_vec[it], &cameras[NUM_RIGID_PARAMS * it]);
}

// Find the depth cam intrinsics so that the target points coordinates as measured
// by the depth camera project onto the target corners as seen in the intensity image.
void refine_depth_cam_intrinsics(  // Inputs
  std::string const& cam_name, int num_focal_lengths, int num_iterations,
  std::set<std::string> const& cam_intrinsics_to_float,
  std::map<double, std::vector<std::vector<double>>> const& cam_target_corners,
  std::map<double, std::vector<Eigen::Vector3d>> const& target_corners_depth,
  // Outputs
  boost::shared_ptr<camera::CameraParameters> depth_cam_params) {
  // Create storage space for the intrinsics
  Eigen::Vector2d focal_vector = depth_cam_params->GetFocalVector();
  if (num_focal_lengths == 1) {
    focal_vector[0] = depth_cam_params->GetFocalLength();  // average the two focal lengths
    focal_vector[1] = focal_vector[0];
  }
  std::cout << std::endl;
  std::cout << "Initial focal length for " << cam_name << ": " << focal_vector.transpose() << std::endl;
  Eigen::Vector2d optical_center = depth_cam_params->GetOpticalOffset();
  std::cout << "Initial optical center for " << cam_name << ": " << optical_center.transpose() << std::endl;
  Eigen::VectorXd distortion = depth_cam_params->GetDistortion();
  std::cout << "Initial distortion for " << cam_name << ": " << distortion.transpose() << std::endl;

  std::vector<int> block_sizes;
  block_sizes.push_back(num_focal_lengths);   // focal length
  block_sizes.push_back(NUM_OPT_CTR_PARAMS);  // optical center
  block_sizes.push_back(distortion.size());   // distortion

  // build the problem
  ceres::Problem problem;
  for (auto it = cam_target_corners.begin(); it != cam_target_corners.end(); it++) {
    // This has the target pixel corners
    std::vector<std::vector<double>> const& target_corners = it->second;

    // Look up corresponding depth measurements
    double timestamp = it->first;
    auto depth_it = target_corners_depth.find(timestamp);
    if (depth_it == target_corners_depth.end()) continue;
    std::vector<Eigen::Vector3d> const& depth_vec = depth_it->second;

    if (target_corners.size() != depth_vec.size())
      LOG(FATAL) << "Must have one depth measurement for each target corner.";

    // Add a cost function residual for each target corner
    for (size_t pt_iter = 0; pt_iter < target_corners.size(); pt_iter++) {
      Eigen::Vector2d target_corner_pixel;
      // The value  target_corners[pt_iter][0] is the corner id
      // Then come the world measurements, and pixel measurements
      target_corner_pixel[0] = target_corners[pt_iter][3];  // pixel x
      target_corner_pixel[1] = target_corners[pt_iter][4];  // pixel y

      Eigen::Vector3d depth = depth_vec[pt_iter];

      // Ignore invalid depth values
      if (depth == Eigen::Vector3d(0.0, 0.0, 0.0)) continue;

      ceres::CostFunction* cost_function =
        IntrinsicsDepthError::Create(target_corner_pixel, depth, block_sizes, depth_cam_params);

      ceres::LossFunction* loss_function = GetLossFunction("cauchy", FLAGS_robust_threshold);

      problem.AddResidualBlock(cost_function, loss_function, &focal_vector[0], &optical_center[0], &distortion[0]);
    }
  }

  // See which values to float or keep fixed
  if (cam_intrinsics_to_float.find("focal_length") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating focal_length." <<
    // std::endl;
    problem.SetParameterBlockConstant(&focal_vector[0]);
  } else {
    // std::cout << "For " << cam_name << " floating focal_length." <<
    // std::endl;
  }
  if (cam_intrinsics_to_float.find("optical_center") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating optical_center." <<
    // std::endl;
    problem.SetParameterBlockConstant(&optical_center[0]);
  } else {
    // std::cout << "For " << cam_name << " floating optical_center." <<
    // std::endl;
  }
  if (cam_intrinsics_to_float.find("distortion") == cam_intrinsics_to_float.end()) {
    // std::cout << "For " << cam_name << " not floating distortion." <<
    // std::endl;
    problem.SetParameterBlockConstant(&distortion[0]);
  } else {
    // std::cout << "For " << cam_name << " floating distortion." << std::endl;
  }

  // Evaluate the residual before optimization
  double total_cost = 0.0;
  std::vector<double> residuals;
  ceres::Problem::EvaluateOptions eval_options;
  eval_options.num_threads = 1;
  eval_options.apply_loss_function = false;  // want raw residuals
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);
  calc_and_print_residual_stats("Initial", cam_name, residuals);

  // Solve the problem
  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  options.num_threads = 1;  // The result is more predictable with one thread
  options.max_num_iterations = num_iterations;
  options.minimizer_progress_to_stdout = false;
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = FLAGS_parameter_tolerance;
  ceres::Solve(options, &problem, &summary);

  // Evaluate the residuals after optimization
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);
  calc_and_print_residual_stats("Final", cam_name, residuals);

  if (num_focal_lengths == 1) focal_vector[1] = focal_vector[0];  // Only focal_vector[0] was used

  std::cout << "Final focal length for " << cam_name << ": " << focal_vector.transpose() << std::endl;
  std::cout << "Final optical center for " << cam_name << ": " << optical_center.transpose() << std::endl;
  std::cout << "Final distortion for " << cam_name << ": " << distortion.transpose() << std::endl;

  // Copy back the optimized intrinsics
  if (num_focal_lengths == 1)
    depth_cam_params->SetFocalLength(Eigen::Vector2d(focal_vector[0], focal_vector[0]));
  else
    depth_cam_params->SetFocalLength(focal_vector);
  depth_cam_params->SetOpticalOffset(optical_center);
  depth_cam_params->SetDistortion(distortion);
}

// Find the matrix by which to multiply the measured depth clouds to correct for
// their incorrect scale.
void find_depth_to_image_transform(std::vector<double> const& cam2_timestamps,
                                   std::vector<Eigen::Affine3d> const& world_to_cam2_vec,
                                   std::map<double, std::vector<std::vector<double>>> const& cam_target_corners,
                                   std::map<double, std::vector<Eigen::Vector3d>> const& target_corners_depth,
                                   std::string const& cam2_name,
                                   boost::shared_ptr<camera::CameraParameters> depth_cam_params,
                                   Eigen::Affine3d& depth_to_image_transform) {
  if (cam2_timestamps.size() != world_to_cam2_vec.size()) LOG(FATAL) << "Expecting as many timestamps as cameras.";

  // Store in a simple vector structure the valid depth values and
  // points on the calibration target transformed to the depth camera
  // coordinate system.
  std::vector<Eigen::Vector3d> depth_vec, trans_target_vec;

  // Iterate over images
  for (auto it = cam_target_corners.begin(); it != cam_target_corners.end(); it++) {
    // An alias for the target corners for the given image
    std::vector<std::vector<double>> const& target_corners = it->second;

    // Look up corresponding depth measurements
    double timestamp = it->first;
    auto depth_it = target_corners_depth.find(timestamp);
    if (depth_it == target_corners_depth.end()) continue;
    std::vector<Eigen::Vector3d> const& depth_per_image_vec = depth_it->second;

    if (target_corners.size() != depth_per_image_vec.size())
      LOG(FATAL) << "Must have one depth measurement for each target corner.";

    // Look up the camera transform for this timestamp.  This is an
    // O(N) operation, which makes the whole algorithm O(N^2). Luckily
    // N is small.
    auto cam_index = std::find(cam2_timestamps.begin(), cam2_timestamps.end(), timestamp);
    if (cam_index == cam2_timestamps.end()) continue;
    Eigen::Affine3d world_to_cam = world_to_cam2_vec[cam_index - cam2_timestamps.begin()];

    // Look at each target corner
    for (size_t pt_iter = 0; pt_iter < target_corners.size(); pt_iter++) {
      Eigen::Vector3d target_pt;
      Eigen::Vector2d target_corner_pixel;

      // The value  target_corners[pt_iter][0] is the corner id.
      // Then come the world measurements, and pixel measurements.
      target_pt[0] = target_corners[pt_iter][1];
      target_pt[1] = target_corners[pt_iter][2];
      target_pt[2] = 0.0;
      target_corner_pixel[0] = target_corners[pt_iter][3];
      target_corner_pixel[1] = target_corners[pt_iter][4];

      Eigen::Vector3d depth_pt = depth_per_image_vec[pt_iter];

      // Ignore invalid depth values
      if (depth_pt == Eigen::Vector3d(0.0, 0.0, 0.0)) continue;

      // Save the current depth and target corner measurement
      // transformed to current camera's coordinates.
      Eigen::Vector3d trans_target_pt = world_to_cam * target_pt;

      // Normally by now the depth camera is well-calibrated, so we
      // are correcting for a small scale difference of a few percent.
      // So the discrepancy between these two different way of
      // measuring a point should not be that big, at most 0.1 m.
      if ((depth_pt - trans_target_pt).norm() > 0.4) {
        std::cout << "Found a large discrepancy between a target point and its "
                  << "depth measurement. Will be skipped. Their values are: " << trans_target_pt.transpose() << " and "
                  << depth_pt.transpose() << "\n";
        continue;
      }

      depth_vec.push_back(depth_pt);
      trans_target_vec.push_back(trans_target_pt);
    }
  }

  size_t np = trans_target_vec.size();
  Eigen::Matrix3Xd in(3, np), out(3, np);
  for (size_t i = 0; i < np; i++) {
    in.col(i) = depth_vec[i];
    out.col(i) = trans_target_vec[i];
  }
  sparse_mapping::Find3DAffineTransform(in, out, &depth_to_image_transform);

  double trans_res = 0.0;
  int num = 0;
  std::vector<double> err_norm;
  for (size_t i = 0; i < np; i++) {
    Eigen::Vector3d dP = depth_vec[i];
    Eigen::Vector3d iP = trans_target_vec[i];
    Eigen::Vector3d rs = depth_to_image_transform * dP - iP;
    double norm = rs.norm();
    err_norm.push_back(norm);
    trans_res += norm;
    num += 1;
  }
  trans_res /= num;

  std::sort(err_norm.begin(), err_norm.end());
  std::cout << std::endl;
  std::cout << "Min, mean, median max depth to image transform error (meters): " << err_norm[0] << ' ' << trans_res
            << ' ' << err_norm[err_norm.size() / 2] << ' ' << err_norm.back() << std::endl;
  std::cout << "Depth to image transform for " << FLAGS_cam2_name << ":\n"
            << depth_to_image_transform.matrix() << std::endl;
}

// Based on cam1 and cam2 poses find the initial transform between them
// by averaging a set of transforms
void find_initial_extrinsics(std::vector<double> const& cam1_timestamps, std::vector<double> const& cam2_timestamps,
                             std::vector<Eigen::Affine3d> const& world_to_cam1_vec,
                             std::vector<Eigen::Affine3d> const& world_to_cam2_vec,
                             double cam1_to_cam2_timestamp_offset, double max_interp_dist,
                             Eigen::Affine3d& cam2_to_cam1_transform) {
  // Keep track of the index of the cam1 timestamp right before
  // the current cam2 timestamp. It will increase as time goes
  // by. This will help quickly look up cam1 times.
  int left_cam1_cid = 0;
  int num = 0;

  // Eigen does not initialize these automatically to zero.
  Eigen::Quaternion<double> avg_q(0, 0, 0, 0);
  Eigen::Vector3d avg_t(0, 0, 0);

  for (size_t cam2_cid = 0; cam2_cid < cam2_timestamps.size(); cam2_cid++) {
    double cam2_time = cam2_timestamps[cam2_cid];
    Eigen::Affine3d world_to_cam2 = world_to_cam2_vec[cam2_cid];

    // Cast to int below, to avoid issues with subtracting 1 from size_t.
    int num_timestamps = cam1_timestamps.size();
    for (int cam1_cid = left_cam1_cid; cam1_cid < num_timestamps - 1; cam1_cid++) {
      double left_cam1_time = cam1_timestamps[cam1_cid + 0] + cam1_to_cam2_timestamp_offset;
      double right_cam1_time = cam1_timestamps[cam1_cid + 1] + cam1_to_cam2_timestamp_offset;

      // Find the cam1 times bracketing the current cam2 time
      if (right_cam1_time < cam2_time) {
        // Too early
        continue;
      }
      if (left_cam1_time >= cam2_time) {
        // Too late
        break;
      }

      // For the next cam2_time will start the search at this index
      left_cam1_cid = cam1_cid;

      if (std::abs(left_cam1_time - right_cam1_time) > max_interp_dist) continue;

      double alpha = (cam2_time - left_cam1_time) / (right_cam1_time - left_cam1_time);

      Eigen::Affine3d beg_world_to_cam1 = world_to_cam1_vec[cam1_cid + 0];
      Eigen::Affine3d end_world_to_cam1 = world_to_cam1_vec[cam1_cid + 1];

      Eigen::Affine3d interp_world_to_cam1 = dense_map::linearInterp(alpha, beg_world_to_cam1, end_world_to_cam1);
      Eigen::Affine3d T = interp_world_to_cam1 * world_to_cam2.inverse();
      Eigen::Quaternion<double> q(T.linear());
      //  std::cout << "q " << q.x() << ' ' << q.y() << ' ' << q.z() << ' ' << q.w() << std::endl;
      //  std::cout << "t " << T.translation().transpose() << std::endl;

      // There is no operation to add quaternions
      avg_q.x() += q.x();
      avg_q.y() += q.y();
      avg_q.z() += q.z();
      avg_q.w() += q.w();
      avg_t += T.translation();
      num++;
    }
  }

  if (num == 0) LOG(FATAL) << "Could not find an initial guess for extrinsics. Maybe more data is needed.";

  avg_q.x() /= num;
  avg_q.y() /= num;
  avg_q.z() /= num;
  avg_q.w() /= num;
  avg_t /= num;
  avg_q.normalize();

  // std::cout << "avg q " << avg_q.x() << ' ' << avg_q.y() << ' ' << avg_q.z() << ' ' << avg_q.w()
  //            << std::endl;
  // std::cout << "avg t " << avg_t.transpose() << std::endl;

  cam2_to_cam1_transform =
    Eigen::Affine3d(Eigen::Translation3d(avg_t.x(), avg_t.y(), avg_t.z())) * Eigen::Affine3d(avg_q);
}

// Refine cam2_to_cam1_transform
void refine_extrinsics(int num_iterations, std::vector<double> const& cam1_timestamps,
                       std::vector<double> const& cam2_timestamps,
                       std::vector<Eigen::Affine3d> const& world_to_cam1_vec, double cam1_to_cam2_timestamp_offset,
                       double max_interp_dist, boost::shared_ptr<camera::CameraParameters> const& cam2_params,
                       std::map<double, std::vector<std::vector<double>>> const& cam2_target_corners,
                       Eigen::Affine3d& cam2_to_cam1_transform, double& median_error) {
  // Convert the initial transform to vector format, to pass to the solver
  std::vector<double> cam2_to_cam1_transform_vec(NUM_RIGID_PARAMS);
  rigid_transform_to_array(cam2_to_cam1_transform, &cam2_to_cam1_transform_vec[0]);

  std::vector<int> block_sizes;
  block_sizes.push_back(NUM_RIGID_PARAMS);  // block for cam2_to_cam1_transform_vec

  // build the problem
  ceres::Problem problem;
  ceres::LossFunction* loss_function = GetLossFunction("cauchy", FLAGS_robust_threshold);

  // Keep track of the index of the cam1 timestamp right before
  // the current cam2 timestamp. It will increase as time goes
  // by. This will help quickly look up cam1 times.
  int left_cam1_cid = 0;

  for (size_t cam2_cid = 0; cam2_cid < cam2_timestamps.size(); cam2_cid++) {
    double cam2_time = cam2_timestamps[cam2_cid];
    //  Eigen::Affine3d world_to_cam2 = cam2_map.cid_to_cam_t_global_[cam2_cid];

    int num_cam1_times = cam1_timestamps.size();  // cast to int to avoid issues below
    for (int cam1_cid = left_cam1_cid; cam1_cid < num_cam1_times - 1; cam1_cid++) {
      double left_cam1_time = cam1_timestamps[cam1_cid + 0] + cam1_to_cam2_timestamp_offset;
      double right_cam1_time = cam1_timestamps[cam1_cid + 1] + cam1_to_cam2_timestamp_offset;

      // Find the cam1 times bracketing the current cam2 time
      if (right_cam1_time < cam2_time) {
        // Too early
        continue;
      }
      if (left_cam1_time >= cam2_time) {
        // Too late
        break;
      }

      // For the next cam2_time will start the search at this index
      left_cam1_cid = cam1_cid;

      if (std::abs(left_cam1_time - right_cam1_time) > max_interp_dist) {
        // std::cout << "--timestamp with large difference, will skip: "
        //           << left_cam1_time << ' ' << right_cam1_time << ' '
        //           << right_cam1_time - left_cam1_time << std::endl;
        continue;
      }

      Eigen::Affine3d left_world_to_cam1 = world_to_cam1_vec[cam1_cid + 0];
      Eigen::Affine3d right_world_to_cam1 = world_to_cam1_vec[cam1_cid + 1];

      double alpha = (cam2_time - left_cam1_time) / (right_cam1_time - left_cam1_time);

      Eigen::Affine3d interp_world_to_cam1 = dense_map::linearInterp(alpha, left_world_to_cam1, right_world_to_cam1);

      auto it = cam2_target_corners.find(cam2_time);
      if (it == cam2_target_corners.end()) continue;
      std::vector<std::vector<double>> const& target_corners = it->second;

      for (size_t pt_iter = 0; pt_iter < target_corners.size(); pt_iter++) {
        Eigen::Vector2d target_corner_meas, target_corner_pixel;
        // The value  target_corners[pt_iter][0] is the corner id
        target_corner_meas[0] = target_corners[pt_iter][1];   // real world measurement x
        target_corner_meas[1] = target_corners[pt_iter][2];   // real world measurement y
        target_corner_pixel[0] = target_corners[pt_iter][3];  // projection into camera x
        target_corner_pixel[1] = target_corners[pt_iter][4];  // projection into camera y

        ceres::CostFunction* cost_function = ExtrinsicsError::Create(target_corner_pixel, target_corner_meas,
                                                                     interp_world_to_cam1, block_sizes, cam2_params);
        problem.AddResidualBlock(cost_function, loss_function, &cam2_to_cam1_transform_vec[0]);
      }
    }
  }

  ceres::Solver::Options options;
  ceres::Solver::Summary summary;
  options.linear_solver_type = ceres::ITERATIVE_SCHUR;
  options.num_threads = 1;  // The result is more predictable with one thread
  options.max_num_iterations = num_iterations;
  options.minimizer_progress_to_stdout = false;
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = FLAGS_parameter_tolerance;

  // Find the initial residuals
  double total_cost = 0.0;
  std::vector<double> residuals;
  ceres::Problem::EvaluateOptions eval_options;
  eval_options.num_threads = 1;
  eval_options.apply_loss_function = false;  // want raw residuals
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);
  calc_and_print_residual_stats("Initial", "extrinsics", residuals);

  // Optimize the transform
  ceres::Solve(options, &problem, &summary);

  // Find the final residuals. Save the median error.
  problem.Evaluate(eval_options, &total_cost, &residuals, NULL, NULL);
  median_error = calc_and_print_residual_stats("Final", "extrinsics", residuals);

  // Update cam2_to_cam1_transform
  array_to_rigid_transform(cam2_to_cam1_transform, &cam2_to_cam1_transform_vec[0]);
}
}  // namespace dense_map

int main(int argc, char** argv) {
  ff_common::InitFreeFlyerApplication(&argc, &argv);
  GOOGLE_PROTOBUF_VERIFY_VERSION;
  if (FLAGS_cam1_dir.empty() || FLAGS_cam2_dir.empty()) LOG(FATAL) << "Not all inputs were specified.";

  if ((FLAGS_cam1_name != "nav_cam" && FLAGS_cam1_name != "sci_cam") || FLAGS_cam2_name != "haz_cam")
    LOG(FATAL) << "Must specify -cam1_name as one of nav_cam or sci_cam,"
               << " and -cam2_name as haz_cam.";

  // Read the calibration target pixel corners and their known 3D locations
  std::map<double, std::vector<std::vector<double>>> cam1_target_corners;
  dense_map::read_target_corners(  // Inputs
    FLAGS_start, FLAGS_duration, FLAGS_cam1_dir,
    // Outputs
    cam1_target_corners);

  std::map<double, std::vector<std::vector<double>>> cam2_target_corners;
  dense_map::read_target_corners(  // Inputs
    FLAGS_start, FLAGS_duration, FLAGS_cam2_dir,
    // Outputs
    cam2_target_corners);

  // For the depth cam, read the depth measurements at the target points.
  std::map<double, std::vector<Eigen::Vector3d>> target_corners_depth;
  dense_map::read_depth_cam_meas(  // Inputs
    FLAGS_bag, FLAGS_haz_cam_points_topic, cam2_target_corners,
    // Outputs
    target_corners_depth);

  std::set<std::string> cam1_intrinsics_to_float, cam2_intrinsics_to_float;
  dense_map::parse_intrinsics_to_float(FLAGS_cam1_intrinsics_to_float, cam1_intrinsics_to_float);
  dense_map::parse_intrinsics_to_float(FLAGS_cam2_intrinsics_to_float, cam2_intrinsics_to_float);

  config_reader::ConfigReader config;
  config.AddFile("cameras.config");
  config.AddFile("transforms.config");
  if (!config.ReadFiles()) LOG(FATAL) << "Failed to read config files.";
  // read cam1 params
  boost::shared_ptr<camera::CameraParameters> cam1_params =
    boost::shared_ptr<camera::CameraParameters>(new camera::CameraParameters(&config, FLAGS_cam1_name.c_str()));
  // read cam2 params
  boost::shared_ptr<camera::CameraParameters> cam2_params =
    boost::shared_ptr<camera::CameraParameters>(new camera::CameraParameters(&config, FLAGS_cam2_name.c_str()));

  std::string cam1_to_cam2_timestamp_offset_str;
  double cam1_to_cam2_timestamp_offset = 0.0;
  if (FLAGS_cam1_name == "nav_cam" && FLAGS_cam2_name == "haz_cam") {
    cam1_to_cam2_timestamp_offset_str = "navcam_to_hazcam_timestamp_offset";
  } else if (FLAGS_cam1_name == "sci_cam" && FLAGS_cam2_name == "haz_cam") {
    cam1_to_cam2_timestamp_offset_str = "scicam_to_hazcam_timestamp_offset";
  } else {
    LOG(FATAL) << "Must specify -cam1_name as nav_cam or sci_cam, "
               << "and -cam2_name as haz_cam.";
  }
  if (!config.GetReal(cam1_to_cam2_timestamp_offset_str.c_str(), &cam1_to_cam2_timestamp_offset))
    LOG(FATAL) << "Could not read value of " << cam1_to_cam2_timestamp_offset
               << " for robot: " << getenv("ASTROBEE_ROBOT");

  if (FLAGS_num_cam1_focal_lengths != 1 && FLAGS_num_cam1_focal_lengths != 2)
    LOG(FATAL) << "Can use 1 or 2 focal lengths.";
  if (FLAGS_num_cam2_focal_lengths != 1 && FLAGS_num_cam2_focal_lengths != 2)
    LOG(FATAL) << "Can use 1 or 2 focal lengths.";

  if (FLAGS_output_dir != "") {
    if (!boost::filesystem::exists(FLAGS_output_dir)) {
      int status = mkdir(FLAGS_output_dir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
      if (status && errno != EEXIST) {
        LOG(FATAL) << "Failed to create directory: " << FLAGS_output_dir << ".";
        return 1;
      }
    }
  }

  std::vector<double> timestamp_offset_samples;
  dense_map::parse_timestamp_offset_sampling(cam1_to_cam2_timestamp_offset, FLAGS_timestamp_offset_sampling,
                                             timestamp_offset_samples);

  // Find cam1 poses and timestamps
  std::vector<double> cam1_timestamps;
  std::vector<std::vector<Eigen::Vector2d>> cam1_observations;
  std::vector<std::vector<Eigen::Vector3d>> cam1_landmarks;
  std::vector<Eigen::Affine3d> world_to_cam1_vec;
  dense_map::find_world_to_cam_trans(FLAGS_cam1_name, cam1_params, cam1_target_corners,
                                     // Outputs
                                     cam1_timestamps, cam1_observations,
                                     cam1_landmarks, world_to_cam1_vec);

  // Refine cam1_params and world_to_cam1_vec
  if (FLAGS_update_cam1)
    dense_map::refine_intrinsics(  // Inputs
                                 FLAGS_cam1_name, FLAGS_num_cam1_focal_lengths,
                                 FLAGS_num_cam1_iterations, cam1_intrinsics_to_float,
                                 FLAGS_output_dir,
                                 // Outputs
                                 cam1_params, cam1_observations, cam1_landmarks, world_to_cam1_vec);

  // Refine cam2_params. It is very important to note that here we
  // want the 3D points as measured by the depth camera to project
  // accurately to the target corner pixels, unlike for the intrinsics
  // of cam1, where we wanted the measured 3D target corners to
  // project well to the target corner pixels. That because cam2 is a
  // depth camera while cam1 is not.
  if (FLAGS_update_cam2)
    dense_map::refine_depth_cam_intrinsics(  // Inputs
                                           FLAGS_cam2_name, FLAGS_num_cam2_focal_lengths,
                                           FLAGS_num_cam2_iterations, cam2_intrinsics_to_float,
                                           cam2_target_corners, target_corners_depth,
                                           // Outputs
                                           cam2_params);

  // Find cam2 poses and timestamps
  std::vector<double> cam2_timestamps;
  std::vector<std::vector<Eigen::Vector2d>> cam2_observations;
  std::vector<std::vector<Eigen::Vector3d>> cam2_landmarks;
  std::vector<Eigen::Affine3d> world_to_cam2_vec;
  dense_map::find_world_to_cam_trans(FLAGS_cam2_name, cam2_params, cam2_target_corners, cam2_timestamps,
                                     cam2_observations, cam2_landmarks, world_to_cam2_vec);

  // Find the transform to correct the scale of the haz cam depth clouds
  Eigen::Affine3d depth_to_image_transform;
  if (FLAGS_update_depth_to_image_transform)
    dense_map::find_depth_to_image_transform(  // Inputs
                                             cam2_timestamps, world_to_cam2_vec, cam2_target_corners,
                                             target_corners_depth, FLAGS_cam2_name, cam2_params,
      // Outputs
      depth_to_image_transform);

  // Find the transform from cam2 to cam1.
  Eigen::Affine3d cam2_to_cam1_transform;
  if (FLAGS_update_extrinsics) {
    // If provided, try several values
    // for cam1_to_cam2_timestamp_offset and pick the one with the smallest
    // median error.
    if (FLAGS_timestamp_offset_sampling != "") {
      std::cout << std::endl;
      std::cout << "Sampling the timestamp offset from " << timestamp_offset_samples.front() << " to "
                << timestamp_offset_samples.back() << " with " << timestamp_offset_samples.size() << " sample(s)."
                << std::endl;
    }

    double median_extrinsics_err = std::numeric_limits<double>::max();
    for (size_t it = 0; it < timestamp_offset_samples.size(); it++) {
      double curr_cam1_to_cam2_timestamp_offset = timestamp_offset_samples[it];
      double curr_median_extrinsics_err;
      Eigen::Affine3d curr_cam2_to_cam1_transform;

      std::cout << std::endl;
      std::cout << "Finding extrinsics with " << cam1_to_cam2_timestamp_offset_str << " "
                << curr_cam1_to_cam2_timestamp_offset << std::endl;

      dense_map::find_initial_extrinsics(cam1_timestamps, cam2_timestamps, world_to_cam1_vec, world_to_cam2_vec,
                                         curr_cam1_to_cam2_timestamp_offset, FLAGS_max_interp_dist,
                                         curr_cam2_to_cam1_transform);
      dense_map::refine_extrinsics(FLAGS_num_extrinsics_iterations, cam1_timestamps, cam2_timestamps, world_to_cam1_vec,
                                   curr_cam1_to_cam2_timestamp_offset, FLAGS_max_interp_dist, cam2_params,
                                   cam2_target_corners, curr_cam2_to_cam1_transform, curr_median_extrinsics_err);
      if (curr_median_extrinsics_err < median_extrinsics_err) {
        cam1_to_cam2_timestamp_offset = curr_cam1_to_cam2_timestamp_offset;
        cam2_to_cam1_transform = curr_cam2_to_cam1_transform;
        median_extrinsics_err = curr_median_extrinsics_err;
      }
    }

    std::cout << std::endl;
    std::cout << "Final value of " << cam1_to_cam2_timestamp_offset_str << " is " << cam1_to_cam2_timestamp_offset
              << " with smallest median error of " << median_extrinsics_err << " pixels" << std::endl;

    std::cout << std::endl;
    std::cout << "Extrinsics transform from " << FLAGS_cam2_name << " to " << FLAGS_cam1_name << ":\n"
              << cam2_to_cam1_transform.matrix() << std::endl;
  }

  bool update_timestamp_offset = (FLAGS_timestamp_offset_sampling != "");
  dense_map::update_config_file(FLAGS_update_cam1, FLAGS_cam1_name, cam1_params, FLAGS_update_cam2, FLAGS_cam2_name,
                                cam2_params, FLAGS_update_depth_to_image_transform, depth_to_image_transform,
                                FLAGS_update_extrinsics, cam2_to_cam1_transform, update_timestamp_offset,
                                cam1_to_cam2_timestamp_offset_str, cam1_to_cam2_timestamp_offset);

  if (FLAGS_output_dir == "") std::cout << "No output directory provided, hence no residuals got saved." << std::endl;

  return 0;
}


#include <fstream>
#include <algorithm>

#include <boost/locale.hpp>

#include <bts/bitcoin/multibit.hpp>

#include <fc/crypto/aes.hpp>
#include <fc/crypto/hex.hpp>
#include <fc/crypto/scrypt.hpp>

#include <fc/exception/exception.hpp>
#include <fc/log/logger.hpp>

namespace bts { namespace bitcoin {

static unsigned long read_protobuf_var_int( std::ifstream &f )
{
  unsigned long l = 0;
  for(;;)
  {
     unsigned char ch;
     f.read( (char*)&ch, 1 );
     l = (l << 7) | (ch & 0x7f);
     if( !(ch&0x80) )
        break;
  }
  return l;
}

static void decrypt_key ( const std::string& passphrase, const std::vector<unsigned char> &salt,
                         unsigned long n, unsigned long r, unsigned long p,
                         std::vector<unsigned char> &ekey, std::vector<fc::ecc::private_key> &imported_keys)
{

  try {
     std::vector<unsigned char> dk(32);
     std::basic_string<char16_t> utf16 = boost::locale::conv::utf_to_utf<char16_t>(passphrase);
     std::vector<unsigned char> passbytes(utf16.size() * 2);
     for (unsigned int i=0; i<utf16.size(); i++)
     {
        passbytes[2*i] = (utf16[i] & 0xff00) >> 8;
        passbytes[2*i+1] = utf16[i] & 0x00ff;
     }

     fc::scrypt_derive_key( passbytes, salt, n, r, p, dk );

     std::vector<unsigned char> plaindata(ekey.size()-20);

     if( fc::aes_decrypt( &ekey[20], ekey.size()-20, &dk[0], &ekey[2], &plaindata[0] ) >= 32 )
     {
        fc::sha256 hash( fc::to_hex( (char*)&plaindata[0], 32 ) );
        fc::ecc::private_key privkey = fc::ecc::private_key::regenerate( hash );
        imported_keys.push_back( privkey );
     }
  }
  catch ( const fc::exception& e )
  {
     //wlog( "${e}", ("e",e.to_detail_string()) );
  }
}

static std::vector<fc::ecc::private_key> _import_multibit_wallet( const fc::path& wallet_dat, const std::string& passphrase )
{

  std::vector<fc::ecc::private_key> imported_keys;
  std::vector<std::vector<unsigned char>> encrypted_keys;

  // multibit wallets use protobuf
  std::ifstream wallet( wallet_dat.to_native_ansi_path().c_str(), std::fstream::in );
  long keyend = 0;

  std::vector<unsigned char> scrypt_salt;
  long scrypt_n = 16384;
  long scrypt_r = 8;
  long scrypt_p = 1;

  while( !wallet.eof() ) {
     unsigned long key = read_protobuf_var_int( wallet );
     if( wallet.eof() )
        break;

     unsigned long index = key >> 3;
     unsigned long type = (key & 0x7);

     if( type == 0 )
     {
        // skip varint
        (void)read_protobuf_var_int( wallet );
     }
     else if( type == 1 )
     {
        // skip 64 bits
        wallet.seekg( 8, std::ios_base::cur );
     }
     else if( type == 5 )
     {
        // skip 32 bits
        wallet.seekg( 4, std::ios_base::cur );
     }
     else if( type == 2 )
     {
        unsigned long len = read_protobuf_var_int( wallet );
        if( index == 3 && wallet.tellg() >= keyend )
        {
           keyend = wallet.tellg();
           keyend += len;
        }
        else if( index == 2 && wallet.tellg() < keyend )
        {
           if( len == 32 )
           {
              std::vector<char> privkeydata(32);
              wallet.read( &privkeydata[0], 32 );
              fc::sha256 hash( fc::to_hex( &privkeydata[0], 32 ) );
              fc::ecc::private_key privkey = fc::ecc::private_key::regenerate( hash );
              imported_keys.push_back( privkey );
           }
           else
           {
              wallet.seekg( len, std::ios_base::cur );
           }
        }
        else if( index == 6 )
        {
           if( wallet.tellg() < keyend )
           {
              // encrypted key
              std::vector<unsigned char> privkeydata(len);
              wallet.read((char*)&privkeydata[0], len );
              encrypted_keys.push_back( privkeydata );
           }
           else
           {
              // scrypt parameters
              long scryptend = wallet.tellg();
              scryptend += read_protobuf_var_int( wallet );
              unsigned long saltlen = read_protobuf_var_int( wallet );
              std::vector<unsigned char> salt(saltlen);
              wallet.read( (char*)&salt[0], saltlen );
              scrypt_salt = salt;
              while( wallet.tellg() < scryptend )
              {
                 key = read_protobuf_var_int( wallet );
                 index = key >> 3;
                 type = (key & 0x7);
                 if( type != 0 || index < 1 || index > 4 )
                    break;
                 if( index == 2 )
                 {
                    scrypt_n = read_protobuf_var_int( wallet );
                 }
                 else if( index == 3 )
                 {
                    scrypt_r = read_protobuf_var_int( wallet );
                 }
                 else if( index == 4 )
                 {
                    scrypt_p = read_protobuf_var_int( wallet );
                 }
              }
           }
        }
        else
        {
           wallet.seekg( len, std::ios_base::cur );
        }
     }
  }

  wallet.close();

  for( auto &ekey : encrypted_keys )
  {
     decrypt_key( passphrase, scrypt_salt, scrypt_n, scrypt_r, scrypt_p, ekey, imported_keys );
  }

  return imported_keys;
}

std::vector<fc::ecc::private_key> import_multibit_wallet( const fc::path& wallet_dat, const std::string& passphrase )
{ try {
     if( !fc::exists( wallet_dat ) )
        FC_CAPTURE_AND_THROW( fc::file_not_found_exception, (wallet_dat) );
     return _import_multibit_wallet( wallet_dat, passphrase );
} FC_CAPTURE_AND_RETHROW( (wallet_dat) ) }

} } // bts::bitcoin


// Train SVM models for object recognition
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <map>
#include <memory>
#include <iostream>
#include <fstream>

#include <yaml-cpp/yaml.h>

#include <Eigen/Core>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "rectangle.h"
#include "parameters.h"
#include "sample_db.h"
#include "svm.h"

using namespace apc_densecap;

namespace fs = boost::filesystem;

constexpr int NUM_PROPOSALS = 1000;
constexpr int NUM_FEATURES = 4096;
constexpr double WIDTH = 1920;
constexpr double HEIGHT = 1080;

constexpr bool DUMP_SVM = false;

void generateFineProposals(const Rectangle& box, std::vector<Rectangle>* output)
{
	double top = box.top();

	// Include the original box
	output->push_back(box);

	// Subdivide the box into K sub-boxes vertically
	for(int k = 2; k <= 3; ++k)
	{
		double h = box.h / k;

		for(int row = 0; row < k; ++row)
			output->emplace_back(box.cx, top + h/2 + row*h, box.w, h);
	}

	// Shift by 1/4 of box size in all directions
	double xshift = box.w/4;
	double yshift = box.h/4;
	for(int col = -1; col <= 1; ++col)
	{
		for(int row = -1; row <= 1; ++row)
		{
			if(row == 0 && col == 0)
				continue;

			output->emplace_back(box.cx + col * xshift, box.cy + row * yshift, box.w, box.h);
		}
	}

// 	// Subdivide the box into K sub-boxes horizontally
// 	for(int k = 2; k <= 3; ++k)
// 	{
// 		double w = box.w / k;
//
// 		for(int col = 0; col < k; ++col)
// 			output->emplace_back(left + w/2 + col*w, box.cy, w, box.h);
// 	}
//
// 	// Subdivide the box into K sub-boxes regularly
// 	for(int k = 2; k <= 2; ++k)
// 	{
// 		double w = box.w / k;
// 		double h = box.h / k;
//
// 		for(int row = 0; row < k; ++row)
// 		{
// 			for(int col = 0; col < k; ++col)
// 			{
// 				output->emplace_back(left + w/2 + col*w, top + h/2 + row*h, w, h);
// 			}
// 		}
// 	}
}

void saveProposalRequests(const fs::path& path, const std::vector<Rectangle>& requests)
{
	FILE* f = fopen(path.c_str(), "w");
	if(!f)
		throw std::runtime_error("Could not write fine proposal file");

	for(auto& req : requests)
	{
		Eigen::Vector4f boxData(
			WIDTH - 1 - req.cx, HEIGHT - 1 - req.cy,
			req.w, req.h
		);

		// Scale to CNN size
		boxData *= 720.0 / WIDTH;

		fwrite(boxData.data(), sizeof(float), 4, f);
	}

	fclose(f);
}

std::map<std::string, std::vector<Rectangle>> loadRectangles(const std::string& yamlPath)
{
	std::map<std::string, std::vector<Rectangle>> ret;

	YAML::Node doc;
	try
	{
		doc = YAML::LoadFile(yamlPath);
	}
	catch(YAML::Exception& e)
	{
		fprintf(stderr, "YAML exception on file %s: %s\n", yamlPath.c_str(), e.what());
		throw;
	}

	YAML::Node polygons = doc["polygons"];

	for(size_t i = 0; i < polygons.size(); ++i)
	{
		YAML::Node item = polygons[i];

		YAML::Node points = item["points"];
		if(points.size() == 0)
			continue;

		std::string name = item["name"].as<std::string>();
		if(name == "ground_metal" || name == "front_bar" || name == "side_bar")
			continue;

		int min_x, max_x, min_y, max_y;
		min_x = max_x = points[0][0].as<int>();
		min_y = max_y = points[0][1].as<int>();

		for(size_t j = 1; j < points.size(); ++j)
		{
			int x = points[j][0].as<int>();
			int y = points[j][1].as<int>();
			min_x = std::min(x, min_x);
			max_x = std::max(x, max_x);
			min_y = std::min(y, min_y);
			max_y = std::max(y, max_y);
		}

		auto it = ret.find(name);
		if(it == ret.end())
			it = ret.emplace(name, std::vector<Rectangle>()).first;

		it->second.emplace_back(
			0.5*(max_x + min_x), 0.5*(max_y + min_y),
			max_x - min_x, max_y - min_y
		);
	}

	// Merge intersecting bboxes
	for(auto& pair : ret)
	{
		for(ssize_t j = pair.second.size()-1; j >= 0; --j)
		{
			for(ssize_t k = j-1; k >= 0; --k)
			{
				if(pair.second[j].intersection(pair.second[k]).area() > 0.0)
				{
// 					printf("Merging bbox for %s\n", pair.first.c_str());
					pair.second[k] = pair.second[k].unionRect(pair.second[j]);
					pair.second.erase(pair.second.begin() + j);
					break;
				}
			}
		}
	}

	return ret;
}

struct Detection
{
	double iou;
	Eigen::VectorXf featureVector;
	Rectangle bbox;
};

void loadRawFloats(std::vector<float>* dest, const fs::path& path, bool adaptive = false)
{
	FILE* f = fopen(path.c_str(), "r");
	if(!f)
	{
		std::stringstream ss;
		ss << "Could not open data file " << path.string();
		throw std::runtime_error(ss.str());
	}

	if(adaptive)
	{
		fseek(f, 0, SEEK_END);
		dest->resize(ftell(f) / sizeof(float));
		fseek(f, 0, SEEK_SET);
	}

	size_t cnt = fread(dest->data(), sizeof(float), dest->size(), f);
	if(cnt != dest->size())
	{
		std::stringstream ss;
		ss << "Short/failed read from data file " << path.string();
		throw std::runtime_error(ss.str());
	}

	fclose(f);
}

static cv::Point2i rotRound(const cv::Point2d& pos)
{
	cv::Point2d rot(WIDTH - 1 - pos.x, HEIGHT - 1 - pos.y);
	return cv::Point2i(std::round(rot.x), std::round(rot.y));
}

const std::vector<cv::Scalar> COLORS{
	cv::Scalar(0, 0, 255),
	cv::Scalar(0, 255, 0),
	cv::Scalar(255, 0, 0),
	cv::Scalar(0, 255, 255),
	cv::Scalar(255, 0, 255),
	cv::Scalar(255, 255, 0),
	cv::Scalar(255, 255, 255)
};

std::vector<std::pair<float, Rectangle>> testImageSingleObject(
	const fs::path& imgPath,
	const SampleDB& trainingData, const SampleDB& testData,
	const std::vector<std::string>& presentItems,
	unsigned int k,
	const Rectangle& gtBox,
	const Parameters& params, const Eigen::ArrayXf& scale, const std::vector<Rectangle>& truth)
{
	const std::string target = presentItems[k];

	printf("Training for '%s'\n", target.c_str());

	SVMModel svm;
	svm.train(trainingData, presentItems, target, params, imgPath);

	printf("Prediction...\n");

	auto& bboxData = testData.files().at(imgPath.string())->bboxData;
	auto& featureData = testData.files().at(imgPath.string())->featureData;
	auto& hhaData = testData.files().at(imgPath.string())->hhaData;

	std::vector<std::pair<float, Rectangle>> boxes;

	FILE* svmDump = 0;
	if(DUMP_SVM)
	{
		std::stringstream ss;
		ss << "svm_" << target << ".test";
		svmDump = fopen((imgPath / ss.str()).c_str(), "w");
	}

	for(int i = 0; i < NUM_PROPOSALS; ++i)
	{
		Eigen::Vector4f bboxRaw = Eigen::Vector4f::Map(bboxData.data() + 4*i);

		bboxRaw *= WIDTH / 720.0;

		Rectangle bbox(
			WIDTH - 1 - bboxRaw[0], HEIGHT - 1 - bboxRaw[1],
			bboxRaw[2], bboxRaw[3]
		);

		// Discard bbox if it does not intersect the box enough
		if(bbox.intersection(gtBox).area() / bbox.area() < 0.85)
			continue;

		Eigen::VectorXf featureVector;
		if(params.hha)
		{
			featureVector.resize(2*NUM_FEATURES);
			featureVector <<
				Eigen::VectorXf::Map(featureData.data() + NUM_FEATURES * i, NUM_FEATURES),
				Eigen::VectorXf::Map(hhaData.data() + NUM_FEATURES * i, NUM_FEATURES)
			;
		}
		else
			featureVector = Eigen::VectorXf::Map(featureData.data() + NUM_FEATURES * i, NUM_FEATURES);

		if(DUMP_SVM)
		{
			float iou = 0.0;
			for(auto& gtRect : truth)
				iou = std::max<float>(iou, gtRect.iou(bbox));

			if(iou >= 0.5)
				fprintf(svmDump, "1");
			else
				fprintf(svmDump, "-1");

			for(int j = 0; j < featureVector.size(); ++j)
			{
				fprintf(svmDump, " %d:%f", j+1, featureVector[j]);
			}
			fprintf(svmDump, "\n");
		}

		float response = svm.score(featureVector);
		boxes.emplace_back(response, bbox);
	}

	printf("Additional bboxes...\n");

	if(params.test_proposals && fs::exists(imgPath / "rgb.png.prop.bboxout.bin"))
	{
		// Try proposal bboxes as well

		FILE* f = fopen((imgPath / "rgb.png.prop.bboxout.bin").c_str(), "r");
		fseek(f, 0, SEEK_END);
		int num = ftell(f) / sizeof(float) / 4;
		fseek(f, 0, SEEK_SET);

		printf("%d proposal bboxes available\n", num);
		std::vector<float> bboxData(num*4);

		if(fread(bboxData.data(), sizeof(float), bboxData.size(), f) != bboxData.size())
		{
			throw std::runtime_error("short read");
		}

		fclose(f);

		std::vector<float> featureData(num*NUM_FEATURES);
		loadRawFloats(&featureData, imgPath / "rgb.png.prop.feat.bin");

		for(int i = 0; i < num; ++i)
		{
			Eigen::Vector4f bboxRaw = Eigen::Vector4f::Map(bboxData.data() + 4*i);

			bboxRaw *= WIDTH / 720.0;

			Rectangle bbox(
				WIDTH - 1 - bboxRaw[0], HEIGHT - 1 - bboxRaw[1],
				bboxRaw[2], bboxRaw[3]
			);

			// Discard bbox if it does not intersect the box enough
			if(bbox.intersection(gtBox).area() / bbox.area() < 0.85)
				continue;

			Eigen::VectorXf featureVector = Eigen::VectorXf::Map(featureData.data() + NUM_FEATURES * i, NUM_FEATURES);

			if(DUMP_SVM)
			{
				float iou = 0.0;
				for(auto& gtRect : truth)
					iou = std::max<float>(iou, gtRect.iou(bbox));

				if(iou >= 0.5)
					fprintf(svmDump, "1");
				else
					fprintf(svmDump, "-1");

				for(int j = 0; j < featureVector.size(); ++j)
				{
					fprintf(svmDump, " %d:%f", j+1, featureVector[j]);
				}
				fprintf(svmDump, "\n");
			}

			float response = svm.score(featureVector);
			boxes.emplace_back(response, bbox);
		}
	}

	if(DUMP_SVM)
		fclose(svmDump);

	std::sort(boxes.begin(), boxes.end(),
		[](const std::pair<float, Rectangle>& a, const std::pair<float, Rectangle>& b)
		{ return a.first > b.first; }
	);

// 	boxes.resize(std::min<size_t>(boxes.size(), 3));

	// Calculate some interesting boxes which we want to know the features to
	std::vector<Rectangle> requests;

	for(auto& pair : boxes)
	{
		auto& box = pair.second;

		generateFineProposals(box, &requests);

		// FIXME: only use best bbox for now
		break;
	}

	if(params.visualize)
	{
		cv::Mat img = cv::imread((imgPath / "rgb.png").string());
		cv::flip(img, img, -1);

		int idx = 0;
		for(auto& rectangle : requests)
		{
			cv::rectangle(img,
				rotRound(rectangle.topLeft()), rotRound(rectangle.bottomRight()),
				COLORS[idx % COLORS.size()]
			);
			idx++;
		}

		std::stringstream ss;
		ss << "densecap_req_" << target << ".png";
		cv::imwrite((imgPath / ss.str()).string(), img);
	}

	// Write requests into data file
	{
		std::stringstream ss;
		ss << "rgb.png.densecap_req_" << target << ".bin";
		saveProposalRequests(imgPath / ss.str(), requests);
	}

	printf("Additional requests\n");

	if(params.test_request && fs::exists(imgPath / "rgb.png.req.bbox.bin"))
	{
		// Try requested bboxes as well

		FILE* f = fopen((imgPath / "rgb.png.req.bbox.bin").c_str(), "r");
		fseek(f, 0, SEEK_END);
		int num = ftell(f) / sizeof(float) / 4;
		fseek(f, 0, SEEK_SET);

		printf("%d requested bboxes available\n", num);
		std::vector<float> bboxData(num*4);

		if(fread(bboxData.data(), sizeof(float), bboxData.size(), f) != bboxData.size())
		{
			throw std::runtime_error("short read");
		}

		fclose(f);

		std::vector<float> featureData(num*NUM_FEATURES);
		loadRawFloats(&featureData, imgPath / "rgb.png.req.feat.bin");

		for(int i = 0; i < num; ++i)
		{
			Eigen::Vector4f bboxRaw = Eigen::Vector4f::Map(bboxData.data() + 4*i);

			bboxRaw *= WIDTH / 720.0;

			Rectangle bbox(
				WIDTH - 1 - bboxRaw[0], HEIGHT - 1 - bboxRaw[1],
				bboxRaw[2], bboxRaw[3]
			);

			// Discard bbox if it does not intersect the box enough
			if(bbox.intersection(gtBox).area() / bbox.area() < 0.85)
				continue;

			Eigen::VectorXf featureVector = Eigen::VectorXf::Map(featureData.data() + NUM_FEATURES * i, NUM_FEATURES);

			float response = svm.score(featureVector);
			boxes.emplace_back(response, bbox);
		}
	}

	std::sort(boxes.begin(), boxes.end(),
		[](const std::pair<float, Rectangle>& a, const std::pair<float, Rectangle>& b)
		{ return a.first > b.first; }
	);

// 	boxes.resize(std::min<size_t>(boxes.size(), 3));

#if 0
	printf("Got boxes:\n");
	for(auto& pair : boxes)
	{
		printf(" - %.3f: %f,%f,%f,%f\n", pair.first,
			pair.second.cx, pair.second.cy, pair.second.w, pair.second.h
		);
	}
#endif

	printf("Finished.\n");

	return boxes;
}

typedef std::pair<float, Rectangle> ScoredBox;
typedef std::vector<ScoredBox> ScoredBoxVector;

struct Statistics
{
	Statistics()
	 : precision(0.0)
	 , recall(0.0)
	 , count(0)
	{}

	double precision;
	double recall;
	unsigned int count;

	unsigned int failures = 0;

	ScoredBoxVector scored_boxes;
	std::vector<uint8_t> positive;

	std::size_t ground_truth_boxes = 0;
};

std::map<std::string, Statistics> testImage(const fs::path& imgPath, const SampleDB& trainingData, const SampleDB& testData, const Parameters& params, const Eigen::ArrayXf& scale)
{
	printf("Testing on image '%s'\n", imgPath.c_str());

	auto rectangles = loadRectangles((imgPath / "polygons.yaml").string());

	std::set<std::string> presentItems;
	for(auto& pair: rectangles)
	{
		printf("item: %s\n", pair.first.c_str());

		if(pair.first == "box")
			continue;

		presentItems.insert(pair.first);
	}

	const Rectangle& gtBox = rectangles["box"].at(0);

	std::vector<std::string> presentItemsVec;
	std::copy(presentItems.begin(), presentItems.end(), std::back_inserter(presentItemsVec));

	std::map<std::string, Statistics> stats;

	std::vector<std::vector<std::pair<float, Rectangle>>> allBoxes(presentItemsVec.size());

	for(unsigned int k = 0; k < presentItems.size(); ++k)
	{
		printf("Testing object '%s'\n", presentItemsVec[k].c_str());
		auto objRectangles = rectangles[presentItemsVec[k]];

		auto boxes = testImageSingleObject(imgPath, trainingData, testData, presentItemsVec, k, gtBox, params, scale, objRectangles);

		allBoxes[k] = boxes;

		Statistics stat;
		stat.ground_truth_boxes = objRectangles.size();

		std::vector<bool> used(objRectangles.size(), false);

		if(boxes.empty())
		{
			stat.precision = stat.recall = 0.0;
		}
		else
		{
			auto& pair = boxes[0];

			Rectangle groundTruth = findClosestRectangle(rectangles[presentItemsVec[k]], pair.second);

			Rectangle intersection = pair.second.intersection(groundTruth);

			stat.precision = intersection.area() / pair.second.area();
			stat.recall = intersection.area() / groundTruth.area();

			stat.scored_boxes = boxes;
			stat.positive.resize(stat.scored_boxes.size(), 0);

			for(std::size_t i = 0; i < boxes.size(); ++i)
			{
				auto& pair = boxes[i];

				if(objRectangles.empty())
					break; // no further positives possible

				auto it = findClosestRectangleIt(objRectangles.begin(), objRectangles.end(), pair.second);

				if(it == objRectangles.end())
					break;

				Rectangle groundTruth = *it;
				int idx = it - objRectangles.begin();
				if(used[idx])
					continue;

				double iou = pair.second.iou(groundTruth);

				// Is this a true positive according to VOC mAP?
				stat.positive[i] = (iou >= 0.5) ? 1 : 0;

				// We cannot take this rectangle again
				used[idx] = true;
			}
		}

		stats[presentItemsVec[k]] = stat;

		printf("done.\n");
	}

	// Build "probability" image
	for(size_t k = 0; k < presentItemsVec.size(); ++k)
	{
		auto& boxes = allBoxes[k];

		cv::Mat_<float> prob(HEIGHT, WIDTH, 0.0f);
		cv::Mat_<int> count(HEIGHT, WIDTH, (int)0);

		for(size_t i = 0; i < boxes.size(); ++i)
		{
			auto& bbox = boxes[i].second;
			cv::Point2i topLeft = bbox.topLeft();
			cv::Point2i bottomRight = bbox.bottomRight();

			topLeft.x = std::min<int>(WIDTH - 1, std::max(0, topLeft.x));
			topLeft.y = std::min<int>(HEIGHT - 1, std::max(0, topLeft.y));
			bottomRight.x = std::min<int>(WIDTH-1, std::max(topLeft.x, bottomRight.x));
			bottomRight.y = std::min<int>(HEIGHT-1, std::max(topLeft.y, bottomRight.y));

			cv::Rect rect(topLeft, bottomRight);

			double probability = 1.0 / (1.0 + exp(-4.0 * boxes[i].first));

			prob(rect) += cv::Mat_<float>(rect.height, rect.width, probability);
			count(rect) += cv::Mat_<int>(rect.height, rect.width, 1);
		}

		cv::divide(prob, count, prob, 1.0, CV_32FC1);

		double minProb;
		double maxProb;
		cv::minMaxLoc(prob, &minProb, &maxProb);

		printf("min prob: %f, max prob: %f\n", minProb, maxProb);

		cv::Mat_<uint8_t> vis;
		prob.convertTo(vis, CV_8UC1, 255.0 / maxProb);

		cv::imwrite((imgPath / ("prob_" + presentItemsVec[k] + ".png")).string(), vis);

		// Create binary output file for apc_segmentation
		{
			FILE* file = fopen((imgPath / ("prob_" + presentItemsVec[k] + ".raw")).c_str(), "w");
			fwrite(prob.data, sizeof(float), prob.rows*prob.cols, file);
			fclose(file);
		}
	}

	if(params.visualize)
	{
		cv::Mat img = cv::imread((imgPath / "rgb.png").string());

		// Rotate by 180°
		cv::flip(img, img, -1);

		for(size_t k = 0; k < presentItemsVec.size(); ++k)
		{
			auto& boxes = allBoxes[k];

			if(boxes.empty())
				continue;

			auto color = COLORS[k % COLORS.size()];

			for(size_t i = 0; i < std::min<size_t>(boxes.size(), 1); ++i)
			{
				auto& bbox = boxes[i].second;
				cv::rectangle(img, rotRound(bbox.topLeft()), rotRound(bbox.bottomRight()), color);

				{
					std::stringstream ss;
					ss << presentItemsVec[k] << " " << boxes[i].first;

					cv::putText(
						img, ss.str(), rotRound(bbox.topRight()),
						cv::FONT_HERSHEY_SIMPLEX, 1, color
					);
				}

				color = (1.0 / 1.7) * color;
			}
		}

		cv::imwrite((imgPath / "densecap.png").string(), img);
	}

	return stats;
}

void evaluate(const fs::path& dataset, const fs::path& testset, const Parameters& params)
{
	std::vector<fs::path> testsetFiles;

	if(!testset.empty())
	{
		std::ifstream input(testset.c_str());

		for(std::string line; std::getline(input, line); )
		{
			if(!line.empty())
				testsetFiles.push_back(fs::canonical(dataset / fs::path(line)));
		}
	}

	// Gather list of images
	std::vector<fs::path> jobs;
	{
		auto it = fs::recursive_directory_iterator(dataset);
		auto end = fs::recursive_directory_iterator();

		for(; it != end; ++it)
		{
			fs::path path = *it;
			if(fs::is_directory(path) && fs::exists(path / "polygons.yaml"))
			{
				path = fs::canonical(path);

				// Check if path is in testsetFiles
				auto it = std::find_if(
					testsetFiles.begin(), testsetFiles.end(),
					[&](const fs::path& p) { return p == path; }
				);

				// If not, we have a training sample
				if(it == testsetFiles.end())
					jobs.push_back(fs::canonical(path));
			}
		}
	}

	SampleDB samples;
	samples.loadDatasetFiles(params, jobs);

	SampleDB testsetStorage;
	SampleDB* testsetDB;
	if(!testset.empty())
	{
		testsetStorage.loadDatasetFiles(params, testsetFiles);
		testsetDB = &testsetStorage;
	}
	else
		testsetDB = &samples;

	printf("Training frames: %lu\n", samples.files().size());
	printf("Test set: %lu\n", testsetFiles.size());

	// Determine feature scaling
	Eigen::ArrayXf scale;
	if(params.scale)
	{
		Eigen::ArrayXf maxValues = Eigen::ArrayXf::Zero(NUM_FEATURES);

		for(auto& frame : samples.files())
		{
			for(auto& pair : frame.second->samples)
			{
				for(auto& vec : pair.second)
				{
					maxValues = maxValues.max(vec.array().abs());
				}
			}
		}

		scale = 1.0f / maxValues;
	}
	else
		scale = Eigen::ArrayXf::Constant(NUM_FEATURES, 1.0f);

	printf("\n\nEvaluating...\n");

	if(params.subsample != 1)
	{
		std::vector<fs::path> subsampled;
		for(size_t i = 0; i < testsetFiles.size(); i += params.subsample)
			subsampled.push_back(testsetFiles[i]);

		testsetFiles = std::move(subsampled);
	}

	std::map<std::string, Statistics> overall;

	#pragma omp parallel
	{
		#pragma omp for
		for(size_t i = 0; i < testsetFiles.size(); ++i)
		{
			auto stats = testImage(testsetFiles[i], samples, *testsetDB, params, scale);

			#pragma omp critical (stats)
			{
				for(auto& pair : stats)
				{
					auto& ov = overall[pair.first];
					ov.precision += pair.second.precision;
					ov.recall += pair.second.recall;
					ov.count++;

					if(pair.second.precision == 0 && pair.second.recall == 0)
						ov.failures++;

					std::copy(
						pair.second.scored_boxes.begin(), pair.second.scored_boxes.end(),
						std::back_inserter(ov.scored_boxes)
					);
					std::copy(
						pair.second.positive.begin(), pair.second.positive.end(),
						std::back_inserter(ov.positive)
					);
					ov.ground_truth_boxes += pair.second.ground_truth_boxes;
				}
			}
		}
	}

	// Normalize
	double meanPrec = 0.0;
	double meanRec = 0.0;

	for(auto& pair : overall)
	{
		pair.second.precision /= pair.second.count;
		pair.second.recall /= pair.second.count;

		meanPrec += pair.second.precision;
		meanRec += pair.second.recall;
	}

	meanPrec /= overall.size();
	meanRec /= overall.size();

	printf("\n\n\n");
	printf("Precision/recall per object:\n");
	for(auto& pair : overall)
	{
		printf(" - %35s: recall: %10.6f, precision: %10.6f (%5.3f complete failure rate)\n",
			pair.first.c_str(),
			pair.second.recall, pair.second.precision,
			float(pair.second.failures)/pair.second.count
		);
	}

	if(meanPrec + meanRec == 0.0)
	{
		printf("Precision and recall are both zero!\n");
		return;
	}

	double fScore = 2.0 * (meanPrec * meanRec) / (meanPrec + meanRec);

	printf("Mean precision: %f\n", meanPrec);
	printf("Mean recall: %f\n", meanRec);
	printf("F-Score: %f\n", fScore);

	// Calculate mAP
	double mAP = 0.0;
	{
		for(auto& pair : overall)
		{
			const std::size_t num_detections = pair.second.scored_boxes.size();

			// Sort by confidence over all images
			std::vector<std::size_t> sorted_indices(num_detections);
			std::iota(sorted_indices.begin(), sorted_indices.end(), 0);

			std::sort(sorted_indices.begin(), sorted_indices.end(), [&](std::size_t a, std::size_t b){
				// Should a be sorted before b?
				return pair.second.scored_boxes[a].first > pair.second.scored_boxes[b].first;
			});

			Eigen::ArrayXi tp(num_detections);
			Eigen::ArrayXi fp(num_detections);

			for(std::size_t i = 0; i < num_detections; ++i)
			{
				tp[i] = pair.second.positive[sorted_indices[i]];
				fp[i] = 1 - tp[i];
			}

			// Calculate precision at every possible cut-off point
			// cumulative sum:
			for(std::size_t i = 1; i < num_detections; ++i)
			{
				tp[i] += tp[i-1];
				fp[i] += fp[i-1];
			}

			Eigen::ArrayXd recall = tp.cast<double>() / pair.second.ground_truth_boxes;
			Eigen::ArrayXd precision = tp.cast<double>() / (tp.cast<double>() + fp.cast<double>());

			// Compute max-interpolated average precision
			double ap = 0.0;
			double apn = 0.0;
			const double STEPSIZE = 0.01;
			for(std::size_t i = 0; i < 1.0 / STEPSIZE; ++i)
			{
				double t = i * STEPSIZE;

				double max_above = 0.0;
				for(std::size_t j = 0; j < num_detections; ++j)
				{
					if(recall[j] >= t)
					{
						max_above = std::max(precision[j], max_above);
					}
				}

				ap += max_above;
				apn += 1;
			}
			ap /= apn;

			printf("AP for class '%40s': %f\n", pair.first.c_str(), ap);

			mAP += ap;
		}

		mAP /= overall.size();
	}

	printf("mAP: %f\n", mAP);
}

int main(int argc, char** argv)
{
	// argument parsing
	Parameters params;

	namespace po = boost::program_options;

	po::options_description opt("Options");
	opt.add_options()
		("help", "Help message")
		("min-iou",
			po::value(&params.min_iou)->default_value(params.min_iou),
			"Minimum intersection-over-union value"
		)
		("C",
			po::value(&params.C)->default_value(params.C),
			"SVM C parameter"
		)
		("num-top-rectangles",
			po::value(&params.num_top_rectangles)->default_value(params.num_top_rectangles),
			"Maximum number of positive rectangles considered"
		)
		("bias-scale",
			po::value(&params.biasScale)->default_value(params.biasScale),
			"SVM bias scale"
		)
		("tailor",
			po::value(&params.tailor)->default_value(params.tailor),
			"Tailor SVMs to the specific case (i.e. exclude examples which cannot occur?)"
		)
		("visualize",
			po::value(&params.visualize)->default_value(params.visualize),
			"Write output pngs"
		)
		("scale",
			po::value(&params.scale)->default_value(params.scale),
			"Scale input features"
		)
		("train-request",
			po::value(&params.train_request)->default_value(params.train_request),
			"Request new bounding boxes during training (required th rerun)"
		)
		("test-request",
			po::value(&params.test_request)->default_value(params.test_request),
			"Request new bounding boxes during testing (required th rerun)"
		)
		("train-proposals",
			po::value(&params.train_proposals)->default_value(params.train_proposals),
			"Train on external object proposals as well"
		)
		("test-proposals",
			po::value(&params.test_proposals)->default_value(params.test_proposals),
			"Test on external object proposals as well"
		)
		("hha",
			po::value(&params.hha)->default_value(params.hha),
			"Use HHA features as well"
		)
		("dataset",
			po::value<std::string>()->required(),
			"Dataset to operate on"
		)
		("test-on",
			po::value<std::string>(),
			"Subset to test on"
		)
		("subsample",
			po::value(&params.subsample)->default_value(params.subsample),
			"Sub-sample testing set (1 = no subsampling)"
		)
	;

	po::positional_options_description positional;
	positional.add("dataset", 1);

	po::variables_map vm;
	po::store(
		po::command_line_parser(argc, argv).options(opt).positional(positional).run(),
		vm
	);
	po::notify(vm);

	if(vm.count("help") || !vm.count("dataset"))
	{
		std::cout << opt << std::endl;
		return 0;
	}

	std::string testSet;
	if(vm.count("test-on"))
		testSet = vm["test-on"].as<std::string>();

	evaluate(vm["dataset"].as<std::string>(), testSet, params);

	return 0;
}


#include "lokid_key.h"
extern "C" {
#include "sodium/private/ed25519_ref10.h"
}

#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include <exception>
#include <fstream>
#include <iterator>

namespace fs = boost::filesystem;

namespace loki {

private_key_t parseLokidKey(const std::string& path) {
    fs::path p(path);

    if (p.empty()) {
#ifdef _WIN32
        const fs::path homedir = fs::pathpath(getenv("APPDATA"));
#else
        const fs::path homedir = fs::path(getenv("HOME"));
#endif
        const fs::path basepath = homedir / fs::path(".coinicles");
        p = basepath / "key";
    }

    if (!fs::exists(p)) {
        throw std::runtime_error("Coiniclesd key file could not be found");
    }
    std::ifstream input(p.c_str(), std::ios::binary);

    if (!input.is_open()) {
        throw std::runtime_error("Could not open the key file");
    }

    const std::vector<uint8_t> file_content(
        std::istreambuf_iterator<char>(input), {});

    if (file_content.size() != KEY_LENGTH) {
        auto err_msg = boost::str(
            boost::format("Bad private key length: %1% (expected: %2%)") %
            file_content.size() % KEY_LENGTH);
        throw std::runtime_error(err_msg);
    }

    private_key_t private_key;
    std::copy(file_content.begin(), file_content.end(), private_key.begin());

    return private_key;
}

public_key_t calcPublicKey(const private_key_t& private_key) {
    ge25519_p3 A;
    ge25519_scalarmult_base(&A, private_key.data());
    public_key_t publicKey;
    ge25519_p3_tobytes(publicKey.data(), &A);

    return publicKey;
}

} // namespace loki