// Copyright (c) George Washington University, all rights reserved.  See the
// accompanying LICENSE file for more information.
#undef NDEBUG
#include <assert.h>
#include <Eigen/Eigen>
#include <glog/logging.h>
#include "GetPot"
#include <unistd.h>
#include <iomanip>

#include "etc_common.h"
#include <HAL/Camera/CameraDevice.h>
#include <sdtrack/TicToc.h>
#include <HAL/IMU/IMUDevice.h>
#include <HAL/Messages/Matrix.h>
#include <SceneGraph/SceneGraph.h>
#include <pangolin/pangolin.h>
#include <ba/BundleAdjuster.h>
#include <ba/InterpolationBuffer.h>
#include <sdtrack/utils.h>
#include "math_types.h"
#include "gui_common.h"
#include "CVars/CVar.h"
#include <thread>
#include "selfcal-cvars.h"
#include "chi2inv.h"

#ifdef CHECK_NANS
#include <xmmintrin.h>
#endif

#include <sdtrack/semi_dense_tracker.h>

#include "online_calibrator.h"

#define POSES_TO_INIT 30

uint32_t keyframe_tracks = UINT_MAX;
double start_time = 0;
uint32_t frame_count = 0;
Sophus::SE3d last_t_ba, prev_delta_t_ba, prev_t_ba;
// Self calibration params
uint32_t num_change_detected = 0;
uint32_t num_change_needed = 3;
bool unknown_cam_calibration = true;
bool unknown_imu_calibration = false;

bool compare_self_cal_with_batch = false;
bool do_self_cal = true;
bool do_imu_self_cal = false;
uint32_t num_self_cal_segments = 5;
uint32_t self_cal_segment_length = 10;

const int window_width = 640 * 1.5;
const int window_height = 480 * 1.5;
std::string g_usage = "SD SELFCAL. Example usage:\n"
    "-cam file:[loop=1]///Path/To/Dataset/[left,right]*pgm -cmod cameras.xml";
bool is_keyframe = true, is_prev_keyframe = true;
bool optimize_landmarks = true;
bool optimize_pose = true;
bool follow_camera = false;
bool is_running = false;
bool is_stepping = false;
bool is_manual_mode = false;
bool do_bundle_adjustment = true;
bool do_start_new_landmarks = true;
bool use_system_time = false;
int image_width;
int image_height;
calibu::Rig<Scalar> rig;
calibu::Rig<Scalar> selfcal_rig;
calibu::Rig<Scalar> aac_rig;
hal::Camera camera_device;
bool has_imu = false;
hal::IMU imu_device;
sdtrack::SemiDenseTracker tracker;

sdtrack::OnlineCalibrator online_calib;
std::vector<pangolin::DataLog> plot_logs;
std::vector<pangolin::Plotter*> plot_views;
std::vector<pangolin::Plotter*> analysis_views;
std::vector<pangolin::DataLog> analysis_logs;

TrackerGuiVars gui_vars;
pangolin::View* params_plot_view;
pangolin::View* imu_plot_view;
pangolin::View* analysis_plot_view;
std::shared_ptr<GetPot> cl;

// TrackCenterMap current_track_centers;
std::list<std::shared_ptr<sdtrack::DenseTrack>>* current_tracks = nullptr;
int last_optimization_level = 0;
// std::shared_ptr<sdtrack::DenseTrack> selected_track = nullptr;
std::shared_ptr<hal::Image> camera_img;
std::vector<std::vector<std::shared_ptr<SceneGraph::ImageView>>> patches;
std::vector<std::shared_ptr<sdtrack::TrackerPose>> poses;
std::vector<std::unique_ptr<SceneGraph::GLAxis>> axes;
std::shared_ptr<SceneGraph::GLPrimitives<>> line_strip;

// Inertial stuff
ba::BundleAdjuster<double, 1, 6, 0> bundle_adjuster;
ba::BundleAdjuster<double, 1, 15, 0> vi_bundle_adjuster;
ba::BundleAdjuster<double, 1, 15, 0> aac_bundle_adjuster;
ba::InterpolationBufferT<ba::ImuMeasurementT<Scalar>, Scalar> imu_buffer;
std::vector<uint32_t> ba_imu_residual_ids, aac_imu_residual_ids;
int orig_num_aac_poses = num_aac_poses;
double prev_cond_error;
int imu_cond_start_pose_id = -1;
int imu_cond_residual_id = -1;
std::shared_ptr<std::thread> aac_thread;
std::mutex aac_mutex;

sdtrack::CalibrationWindow pq_window;
sdtrack::CalibrationWindow candidate_window;
double last_window_kl_divergence = 0;
double last_added_window_kl_divergence = 0;
uint32_t unknown_cam_calibration_start_pose = 0;
uint32_t unknown_imu_calibration_start_pose = 0;
double total_last_frame_proj_norm = 0;

// State variables
std::vector<cv::KeyPoint> keypoints;
Sophus::SE3d guess;

void ImuCallback(const hal::ImuMsg& ref) {
  const double timestamp = use_system_time ? ref.system_time() :
                                             ref.device_time();
  Eigen::VectorXd a, w;
  hal::ReadVector(ref.accel(), &a);
  hal::ReadVector(ref.gyro(), &w);
  imu_buffer.AddElement(ba::ImuMeasurementT<Scalar>(w, a, timestamp));
  // std::cerr << "Added accel: " << a.transpose() << " and gyro " <<
  //              w.transpose() << " at time " << timestamp << std::endl;
}

template <typename BaType>
void DoBundleAdjustment(BaType& ba, bool use_imu,
                        bool do_adaptive_conditioning,
                        uint32_t& num_active_poses, uint32_t id,
                        std::vector<uint32_t>& imu_residual_ids,
                        calibu::Rig<Scalar>& ba_rig)
{
  std::vector<uint32_t> last_frame_proj_residual_ids;
  if (reset_outliers) {
    for (std::shared_ptr<sdtrack::TrackerPose> pose : poses) {
      for (std::shared_ptr<sdtrack::DenseTrack> track: pose->tracks) {
        track->is_outlier = false;
      }
    }
    reset_outliers = false;
  }

  imu_residual_ids.clear();
  ba::Options<double> options;
  options.gyro_sigma = gyro_sigma;
  options.accel_sigma = accel_sigma;
  options.accel_bias_sigma = accel_bias_sigma;
  options.gyro_bias_sigma = gyro_bias_sigma;
  options.use_dogleg = use_dogleg;
  options.use_sparse_solver = true;
  options.param_change_threshold = 1e-10;
  options.error_change_threshold = 1e-3;
  options.use_robust_norm_for_proj_residuals = use_robust_norm_for_proj;
  options.projection_outlier_threshold = outlier_threshold;
  options.use_per_pose_cam_params = true;
  options.regularize_biases_in_batch = poses.size() < POSES_TO_INIT ||
      regularize_biases_in_batch;

  uint32_t num_outliers = 0;
  Sophus::SE3d t_ba;
  uint32_t start_active_pose, start_pose_id;

  uint32_t end_pose_id;
  {
    std::lock_guard<std::mutex> lock(aac_mutex);
    end_pose_id = poses.size() - 1;

    GetBaPoseRange(poses, num_active_poses, start_pose_id, start_active_pose);

    if (start_pose_id == end_pose_id) {
      return;
    }

    // Add an extra pose to conditon the IMU
    if (use_imu && use_imu_measurements && start_active_pose == start_pose_id &&
        start_pose_id != 0) {
      start_pose_id--;
      std::cerr << "expanding sp from " << start_pose_id - 1 << " to " << start_pose_id << std::endl;
    }
  }

  bool all_poses_active = start_active_pose == start_pose_id;

  // Do a bundle adjustment on the current set
  if (current_tracks && end_pose_id) {
    {
      std::lock_guard<std::mutex> lock(aac_mutex);
      if (use_imu) {
        ba.SetGravity(gravity_vector);
      }

      ba.Init(options, end_pose_id + 1, current_tracks->size() *
              (end_pose_id + 1));
      for (uint32_t cam_id = 0; cam_id < ba_rig.cameras_.size(); ++cam_id) {
        ba.AddCamera(ba_rig.cameras_[cam_id]);
      }

      // First add all the poses and landmarks to ba.
      for (uint32_t ii = start_pose_id ; ii <= end_pose_id ; ++ii) {
        std::shared_ptr<sdtrack::TrackerPose> pose = poses[ii];
        pose->opt_id[id] = ba.AddPose(
              pose->t_wp, pose->cam_params, pose->v_w, pose->b,
              ii >= start_active_pose , pose->time);

        if (ii == start_active_pose && use_imu && all_poses_active) {
          ba.RegularizePose(pose->opt_id[id], true, true, false, false);
        }

        if (use_imu && ii >= start_active_pose && ii > 0) {
          std::vector<ba::ImuMeasurementT<Scalar>> meas =
              imu_buffer.GetRange(poses[ii - 1]->time, pose->time);

          /*std::cerr << "Adding imu residual between poses " << ii - 1 << std::setprecision(15) <<
                       " with time " << poses[ii - 1]->time << " active: " <<
                       ba.GetPose(poses[ii - 1]->opt_id[id]).is_active <<
                       " and " << ii << " with time " << pose->time <<
                       " active: " <<
                       ba.GetPose(poses[ii]->opt_id[id]).is_active <<
                       " with " << meas.size() << " measurements" << std::endl;*/

          imu_residual_ids.push_back(
                ba.AddImuResidual(poses[ii - 1]->opt_id[id],
                pose->opt_id[id], meas));
          if (do_adaptive_conditioning) {
            if (imu_cond_start_pose_id == -1 &&
                !ba.GetPose(poses[ii - 1]->opt_id[id]).is_active &&
                ba.GetPose(pose->opt_id[id]).is_active) {
              // std::cerr << "Setting cond pose id to " << ii - 1 << std::endl;
              imu_cond_start_pose_id = ii - 1;
              imu_cond_residual_id = imu_residual_ids.back();
              // std::cerr << "Setting cond residual id to " <<
              //              imu_cond_residual_id << std::endl;
            } else if ((uint32_t)imu_cond_start_pose_id == ii - 1) {
              imu_cond_residual_id = imu_residual_ids.back();
              // std::cerr << "Setting cond residual id to " <<
              //              imu_cond_residual_id << std::endl;
            }
          }
        }

        for (std::shared_ptr<sdtrack::DenseTrack> track: pose->tracks) {
          const bool constrains_active =
              track->keypoints.size() + ii > start_active_pose;
          if (track->num_good_tracked_frames <= 1 || track->is_outlier ||
              !constrains_active) {
            track->external_id[id] = UINT_MAX;
            continue;
          }
          Eigen::Vector4d ray;
          ray.head<3>() = track->ref_keypoint.ray;
          ray[3] = track->ref_keypoint.rho;
          ray = sdtrack::MultHomogeneous(pose->t_wp  * ba_rig.cameras_[0]->Pose(), ray);
          bool active = track->id != tracker.longest_track_id() ||
              !all_poses_active || use_imu;
          if (!active) {
            std::cerr << "Landmark " << track->id << " inactive. " << std::endl;
          }
          track->external_id[id] =
              ba.AddLandmark(ray, pose->opt_id[id], 0, active);
        }
      }

      // Now add all reprojections to ba)
      for (uint32_t ii = start_pose_id ; ii <= end_pose_id ; ++ii) {
        std::shared_ptr<sdtrack::TrackerPose> pose = poses[ii];
        uint32_t total_proj_res = 0;
        for (std::shared_ptr<sdtrack::DenseTrack> track : pose->tracks) {
          if (track->external_id[id] == UINT_MAX) {
            continue;
          }
          for (uint32_t cam_id = 0; cam_id < ba_rig.cameras_.size(); ++cam_id) {
            for (size_t jj = 0; jj < track->keypoints.size() ; ++jj) {
              if (track->keypoints[jj][cam_id].tracked) {
                const Eigen::Vector2d& z = track->keypoints[jj][cam_id].kp;
                if (ba.GetNumPoses() > (pose->opt_id[id] + jj)) {
                  const uint32_t res_id =
                      ba.AddProjectionResidual(
                        z, pose->opt_id[id] + jj,
                        track->external_id[id], cam_id, 2.0);

                  // Store reprojection constraint ids for the last frame.
                  if ((ii + jj) == end_pose_id) {
                    last_frame_proj_residual_ids.push_back(res_id);
                  }
                  total_proj_res++;
                }
              }
            }
          }
        }

        if (!do_adaptive_conditioning) {
          // std::cerr << "Total proj res for pose: " << ii << ": " <<
          //             total_proj_res << std::endl;
        }
      }
    }

    // Optimize the poses
    ba.Solve(num_ba_iterations);

    {
      std::lock_guard<std::mutex> lock(aac_mutex);
      total_last_frame_proj_norm = 0;

      ////ZZZZZZZZZZ THIS IS NOT THREAD SAFE
      // Calculate the average reprojection error.
      for (uint32_t id : last_frame_proj_residual_ids) {
        const auto& res = ba.GetProjectionResidual(id);
        total_last_frame_proj_norm += res.z.norm();
      }
      total_last_frame_proj_norm /= last_frame_proj_residual_ids.size();

      uint32_t last_pose_id =
          is_keyframe ? poses.size() - 1 : poses.size() - 2;
      std::shared_ptr<sdtrack::TrackerPose> last_pose = poses[last_pose_id];

      if (last_pose_id <= end_pose_id) {
        // Get the pose of the last pose. This is used to calculate the relative
        // transform from the pose to the current pose.
        last_pose->t_wp = ba.GetPose(last_pose->opt_id[id]).t_wp;
      }

      // Read out the pose and landmark values.
      for (uint32_t ii = start_pose_id ; ii <= end_pose_id ; ++ii) {
        std::shared_ptr<sdtrack::TrackerPose> pose = poses[ii];
        const ba::PoseT<double>& ba_pose =
            ba.GetPose(pose->opt_id[id]);

        pose->t_wp = ba_pose.t_wp;
        if (use_imu) {
          pose->v_w = ba_pose.v_w;
          pose->b = ba_pose.b;
        }

        // Here the last pose is actually t_wb and the current pose t_wa.
        last_t_ba = t_ba;
        t_ba = last_pose->t_wp.inverse() * pose->t_wp;
        for (std::shared_ptr<sdtrack::DenseTrack> track: pose->tracks) {
          if (track->external_id[id] == UINT_MAX) {
            continue;
          }
          track->t_ba = t_ba;

          // Get the landmark location in the world frame.
          const Eigen::Vector4d& x_w =
              ba.GetLandmark(track->external_id[id]);
          double ratio =
              ba.LandmarkOutlierRatio(track->external_id[id]);

          if (do_outlier_rejection && !unknown_cam_calibration &&
              poses.size() > POSES_TO_INIT) {
            if (ratio > 0.3 && track->tracked == false &&
                (end_pose_id >= min_poses_for_imu - 1 || !use_imu)) {
//            if (ratio > 0.3 &&
//                ((track->keypoints.size() == num_ba_poses - 1) ||
//                 track->tracked == false)) {
              num_outliers++;
              track->is_outlier = true;
            } else {
              track->is_outlier = false;
            }
          }

          Eigen::Vector4d prev_ray;
          prev_ray.head<3>() = track->ref_keypoint.ray;
          prev_ray[3] = track->ref_keypoint.rho;
          // Make the ray relative to the pose.
          Eigen::Vector4d x_r =
              sdtrack::MultHomogeneous(
                (pose->t_wp * ba_rig.cameras_[0]->Pose()).inverse(), x_w);
          // Normalize the xyz component of the ray to compare to the original
          // ray.
          x_r /= x_r.head<3>().norm();
          track->ref_keypoint.rho = x_r[3];
        }
      }

      if (follow_camera) {
        FollowCamera(gui_vars, poses.back()->t_wp);
      }
    }
  }
  if (!do_adaptive_conditioning) {
    std::cerr << "Rejected " << num_outliers << " outliers." << std::endl;
  }

  const ba::SolutionSummary<Scalar>& summary = ba.GetSolutionSummary();
  // std::cerr << "Rejected " << num_outliers << " outliers." << std::endl;

  if (use_imu && imu_cond_start_pose_id != -1 && do_adaptive_conditioning) {
    const uint32_t cond_dims =
        summary.num_cond_inertial_residuals * BaType::kPoseDim +
        summary.num_cond_proj_residuals * 2;
    const Scalar cond_error = summary.cond_inertial_error +
        summary.cond_proj_error;

    const double cond_inertial_error =
        ba.GetImuResidual(
          imu_cond_residual_id).mahalanobis_distance;

    if (prev_cond_error == -1) {
      prev_cond_error = DBL_MAX;
    }

    const Scalar cond_v_chi2_dist =
        chi2inv(adaptive_threshold, summary.num_cond_proj_residuals * 2);
    const Scalar cond_i_chi2_dist =
        chi2inv(adaptive_threshold, BaType::kPoseDim);

    if (num_active_poses > end_pose_id) {
      num_active_poses = orig_num_aac_poses;
      // std::cerr << "Reached batch solution. resetting number of poses to " <<
      //              num_ba_poses << std::endl;
    }

    if (cond_error == 0 || cond_dims == 0) {
      // status = OptStatus_NoChange;
    } else {
      const double cond_total_error =
          (cond_inertial_error + summary.cond_proj_error);
      const double inertial_ratio = cond_inertial_error / cond_i_chi2_dist;
      const double visual_ratio = summary.cond_proj_error / cond_v_chi2_dist;
      if ((inertial_ratio > 1.0 || visual_ratio > 1.0) &&
          (cond_total_error <= prev_cond_error) &&
          (((prev_cond_error - cond_total_error) / prev_cond_error) > 0.00001)) {
        num_active_poses += 30;
      } else {
        num_active_poses = orig_num_aac_poses;

      }
      prev_cond_error = cond_total_error;
    }
  }
}

void UpdateCurrentPose()
{
  std::shared_ptr<sdtrack::TrackerPose> current_pose = poses.back();
  if (poses.size() > 1) {
    current_pose->t_wp = poses[poses.size() - 2]->t_wp * tracker.t_ba().inverse();
  }

  // Also use the current tracks to update the index of the earliest covisible
  // pose.
  size_t max_track_length = 0;
  for (std::shared_ptr<sdtrack::DenseTrack>& track : tracker.GetCurrentTracks()) {
    max_track_length = std::max(track->keypoints.size(), max_track_length);
  }
  current_pose->longest_track = max_track_length;
  std::cerr << "Setting longest track for pose " << poses.size() << " to " <<
               current_pose->longest_track << std::endl;
}

void DoAAC()
{
  while (true) {
    if (has_imu && use_imu_measurements &&
        poses.size() > 10 && do_async_ba) {

      orig_num_aac_poses = num_aac_poses;
      while (true) {
        if (poses.size() > min_poses_for_imu &&
            use_imu_measurements && has_imu) {
          {
            std::lock_guard<std::mutex> lock(aac_mutex);
            aac_rig.cameras_[0]->SetParams(rig.cameras_[0]->GetParams());
            aac_rig.cameras_[0] = rig.cameras_[0];
          }
          DoBundleAdjustment(aac_bundle_adjuster, true, do_adaptive,
                             num_aac_poses, 1, aac_imu_residual_ids,
                             aac_rig);
        }

        if ((int)num_aac_poses == orig_num_aac_poses || !do_adaptive) {
          break;
        }

        usleep(10);
      }

      imu_cond_start_pose_id = -1;
      prev_cond_error = -1;
    }
    usleep(1000);
  }
}

void BaAndStartNewLandmarks()
{
  bool imu_selfcal_active = has_imu && use_imu_measurements && do_imu_self_cal;
  if (!is_keyframe) {
    return;
  }

  sdtrack::CalibrationWindow current_window;

  uint32_t keyframe_id = poses.size();
  // If we have no idea about the calibration, do batch mode.
  double batch_time = sdtrack::Tic();
  double ba_time = 0, analyze_time = 0, queue_time = 0, snl_time = 0;
  const uint32_t batch_start = unknown_cam_calibration_start_pose;
  const uint32_t batch_end = poses.size();
  if (do_self_cal && (unknown_cam_calibration || unknown_imu_calibration)
      && ((batch_end - batch_start) > self_cal_segment_length)) {
    bool window_analyzed = false;
    if (imu_selfcal_active && poses.size() > min_poses_for_imu &&
        unknown_imu_calibration) {
      online_calib.AnalyzeCalibrationWindow<true>(
            poses, current_tracks, batch_start,
            batch_end, pq_window, 50, true);
      window_analyzed = true;
    } else if(unknown_cam_calibration) {
      online_calib.AnalyzeCalibrationWindow<false>(
            poses, current_tracks, batch_start,
            batch_end, pq_window, 50, true);
      window_analyzed = true;
    }

    double score = window_analyzed ? online_calib.GetWindowScore(pq_window) : 0;

    if (pq_window.covariance.fullPivLu().rank() ==
        selfcal_rig.cameras_[0]->GetParams().rows() /*&& score < 1e7*/) {
      std::cerr << "Setting new batch params: " << std::endl;
      const Eigen::VectorXd new_params = selfcal_rig.cameras_[0]->GetParams();
      rig.cameras_[0]->SetParams(new_params);
      rig.cameras_[0] = selfcal_rig.cameras_[0];
      {
        std::unique_lock<std::mutex>(aac_mutex);
        for (uint32_t ii = unknown_cam_calibration_start_pose ;
             ii < poses.size() ; ++ii) {
          poses[ii]->cam_params = new_params;
          for (std::shared_ptr<sdtrack::DenseTrack> track: poses[ii]->tracks) {
            if (track->external_id[0] == UINT_MAX) {
              continue;
            }
            // We also have to backproject this track again.
            // tracker.BackProjectTrack(track);
            track->ref_keypoint.ray =
                rig.cameras_[0]->Unproject(
                  track->ref_keypoint.center_px).normalized();
            track->needs_backprojection = true;
          }
        }
      }
    }

    if (pq_window.mean.rows() != 0) {
      current_window = pq_window;
    }

    // Write this to the batch file.
    std::ofstream("batch.txt", std::ios_base::app) << keyframe_id << ", " <<
      pq_window.covariance.diagonal().transpose().format(
      sdtrack::kLongCsvFmt) << ", " << score << ", " <<
      pq_window.mean.transpose().format(sdtrack::kLongCsvFmt) << std::endl;

    std::cerr << "Batch means are: " << pq_window.mean.transpose() <<
                 std::endl;
    std::cerr << "Batch sigmas are:\n" <<
                 pq_window.covariance << std::endl;
    std::cerr << "Batch score: " << score << std::endl;

    // If the determinant is smaller than a heuristic, switch to self_cal.
    if ((score < 1e7 && score != 0 && !std::isnan(score) && !std::isinf(score)) ||
        ((batch_end - batch_start) > self_cal_segment_length * 2)) {
      std::cerr << "Determinant small enough, switching to self-cal" <<
                   std::endl;
      unknown_cam_calibration = false;
      unknown_imu_calibration = false;
    }
  }
  batch_time = sdtrack::Toc(batch_time);

  if (do_bundle_adjustment) {
    ba_time = sdtrack::Tic();
    uint32_t ba_size = std::max(num_ba_poses, unknown_cam_calibration ?
                                  batch_end - batch_start : num_ba_poses);
    if (has_imu && use_imu_measurements &&
        poses.size() > min_poses_for_imu) {
      std::cerr << "doing VI BA." << std::endl;
      DoBundleAdjustment(vi_bundle_adjuster, true, false, ba_size, 0,
                         ba_imu_residual_ids, rig);
    } else {
      std::cerr << "doing visual BA." << std::endl;
      DoBundleAdjustment(bundle_adjuster, false, false, ba_size, 0,
                         ba_imu_residual_ids, rig);
    }
    ba_time = sdtrack::Toc(ba_time);

    if (do_self_cal && (batch_end - batch_start) >= self_cal_segment_length) {
      analyze_time = sdtrack::Tic();
      uint32_t start_pose =
          std::max(0, (int)poses.size() - (int)self_cal_segment_length);
      uint32_t end_pose = poses.size();
      if (imu_selfcal_active) {
        online_calib.AnalyzeCalibrationWindow<true>(
              poses, current_tracks, start_pose, end_pose,
              candidate_window, 50);
      } else {
        online_calib.AnalyzeCalibrationWindow<false>(
              poses, current_tracks, start_pose, end_pose,
              candidate_window, 50);
      }

      online_calib.AnalyzeCalibrationWindow(candidate_window);
      last_window_kl_divergence =
          online_calib.ComputeYao1965(
            pq_window, candidate_window);

      if (candidate_window.mean.rows() != 0) {
        current_window = candidate_window;
      }

      std::cerr << "KL divergence for last window: " <<
                   last_window_kl_divergence << " num change: " <<
                   num_change_detected << std::endl;
      if (isnan(last_window_kl_divergence) ||
          isinf(last_window_kl_divergence)) {
        last_window_kl_divergence = 0;
      }

      if (last_window_kl_divergence < 0.2 && last_window_kl_divergence != 0 &&
          (online_calib.NumWindows() == online_calib.queue_length()) &&
          !unknown_cam_calibration) {
        num_change_detected++;

        if (num_change_detected > num_change_needed) {
          std::cerr << "PARAM CHANGE DETECTED" << std::endl;
          unknown_cam_calibration = true;
          unknown_cam_calibration_start_pose = poses.size() - num_change_needed;
          std::cerr << "Unknown calibration = true with start pose " <<
                       unknown_cam_calibration_start_pose << std::endl;
          online_calib.ClearQueue();
        }
      } else {
        num_change_detected = 0;
      }
      analyze_time = sdtrack::Toc(analyze_time);

      // If the priority queue was modified, calculate the new results for it.
      if (online_calib.needs_update() && !unknown_cam_calibration) {
        queue_time = sdtrack::Toc(ba_time);
        last_added_window_kl_divergence = last_window_kl_divergence;
        const bool apply_results =
            !(unknown_cam_calibration || unknown_imu_calibration);
        if (imu_selfcal_active) {
          online_calib.AnalyzePriorityQueue<true>(
                poses, current_tracks, pq_window, 50, apply_results);
        } else {
          online_calib.AnalyzePriorityQueue<false>(
                poses, current_tracks, pq_window, 50, apply_results);
        }
        if (apply_results) {
          std::unique_lock<std::mutex>(aac_mutex);
          const Eigen::VectorXd new_params =
              selfcal_rig.cameras_[0]->GetParams();
          rig.cameras_[0]->SetParams(new_params);
          for (size_t ii = unknown_cam_calibration_start_pose;
               ii < poses.size(); ++ii) {
            for (std::shared_ptr<sdtrack::DenseTrack> track : poses[ii]->tracks) {
              poses[ii]->cam_params = new_params;
              track->ref_keypoint.ray =
                  rig.cameras_[0]->Unproject(
                    track->ref_keypoint.center_px).normalized();
              // tracker.BackProjectTrack(track);
              track->needs_backprojection = true;
            }
          }
        }
        std::cerr << "Analyzed priority queue with mean " <<
                     pq_window.mean.transpose() << " and cov\n " <<
                     pq_window.covariance << std::endl;
        online_calib.SetPriorityQueueDistribution(pq_window.covariance,
                                                  pq_window.mean);

        const double score =
            online_calib.GetWindowScore(pq_window);

        // Write this to the pq file.
        std::ofstream("pq.txt", std::ios_base::app) << keyframe_id << ", " <<
          pq_window.covariance.diagonal().transpose().format(
            sdtrack::kLongCsvFmt) << ", " << score << ", " <<
          pq_window.mean.transpose().format(sdtrack::kLongCsvFmt) << ", " <<
          last_window_kl_divergence << std::endl;

        if (compare_self_cal_with_batch && !unknown_cam_calibration) {
          // Also analyze the full batch solution.
          sdtrack::CalibrationWindow batch_window;
          if (imu_selfcal_active) {
            online_calib.AnalyzeCalibrationWindow<true>(
                  poses, current_tracks, 0, poses.size(), batch_window, 50);
          } else {
            online_calib.AnalyzeCalibrationWindow<false>(
                  poses, current_tracks, 0, poses.size(), batch_window, 50);
          }

          const double batch_score =
              online_calib.GetWindowScore(batch_window);

          // Write this to the batch file.
          std::ofstream("batch.txt", std::ios_base::app) << keyframe_id << ", " <<
            batch_window.covariance.diagonal().transpose().format(
            sdtrack::kLongCsvFmt) << ", " << batch_score << ", " <<
            batch_window.mean.transpose().format(sdtrack::kLongCsvFmt) <<
            std::endl;

          std::cerr << "Batch means are: " << batch_window.mean.transpose() <<
                       std::endl;
          std::cerr << "Batch sigmas are:\n" <<
                       batch_window.covariance << std::endl;
          std::cerr << "Batch score: " << batch_score << std::endl;
        }

        queue_time = sdtrack::Toc(queue_time);
      }
    }

    if (do_self_cal && current_window.mean.rows() != 0) {
      std::ofstream("sigmas.txt", std::ios_base::app) << keyframe_id << ", " <<
        current_window.covariance.diagonal(). transpose().format(
          sdtrack::kLongCsvFmt) << ", " <<
        current_window.mean.transpose().format(sdtrack::kLongCsvFmt) <<
        ", " << last_window_kl_divergence << ", " << current_window.score <<
        std::endl;
    }
  }

  if (do_start_new_landmarks) {
    snl_time = sdtrack::Tic();
    tracker.StartNewLandmarks();
    snl_time = sdtrack::Toc(snl_time);
  }

  std::cerr << "Timings batch: " << batch_time << " ba: " << ba_time <<
               " analyze: " << analyze_time << " queue: " << queue_time <<
               " snl: " << snl_time << std::endl;

  std::ofstream("timings.txt", std::ios_base::app) << keyframe_id << ", " <<
    batch_time << ", " << ba_time << ", " << analyze_time << ", " <<
    queue_time << ", " << snl_time << std::endl;

  std::shared_ptr<sdtrack::TrackerPose> new_pose = poses.back();
  // Update the tracks on this new pose.
  new_pose->tracks = tracker.GetNewTracks();

  if (!do_bundle_adjustment) {
    tracker.TransformTrackTabs(tracker.t_ba());
  }
}

void ProcessImage(std::vector<cv::Mat>& images, double timestamp)
{
  bundle_adjuster.debug_level_threshold = ba_debug_level;
  vi_bundle_adjuster.debug_level_threshold = vi_ba_debug_level;
  aac_bundle_adjuster.debug_level_threshold = aac_ba_debug_level;
  online_calib.SetBaDebugLevel(selfcal_ba_debug_level);

#ifdef CHECK_NANS
  _MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() &
                         ~(_MM_MASK_INVALID | _MM_MASK_OVERFLOW |
                           _MM_MASK_DIV_ZERO));
#endif

  if (frame_count == 0) {
    start_time = sdtrack::Tic();
  }
  frame_count++;
//  if (poses.size() > 100) {
//    std::cerr << "last_pose\n " << poses.back()->t_wp.matrix().format(
//                   sdtrack::kLongFmt) << std::endl;
//    exit(EXIT_SUCCESS);
//  }

  // If this is a keyframe, set it as one on the tracker.
  prev_delta_t_ba = tracker.t_ba() * prev_t_ba.inverse();

  if (is_prev_keyframe) {
    prev_t_ba = Sophus::SE3d();
  } else {
    prev_t_ba = tracker.t_ba();
  }

  // Add a pose to the poses array
  if (is_prev_keyframe) {
    std::shared_ptr<sdtrack::TrackerPose> new_pose(new sdtrack::TrackerPose);
    if (poses.size() > 0) {
      new_pose->t_wp = poses.back()->t_wp * last_t_ba.inverse();
      if (use_imu_measurements && has_imu) {
        new_pose->v_w = poses.back()->v_w;
        new_pose->b = poses.back()->b;
      }
    } else {
      if (has_imu && use_imu_measurements && imu_buffer.elements.size() > 0) {
        Eigen::Vector3t down = -imu_buffer.elements.front().a.normalized();
        std::cerr << "Down vector based on first imu meas: " <<
                     down.transpose() << std::endl;

        // compute path transformation
        Eigen::Vector3t forward(1.0, 0.0, 0.0);
        Eigen::Vector3t right = down.cross(forward);
        right.normalize();
        forward = right.cross(down);
        forward.normalize();

        Eigen::Matrix4t base = Eigen::Matrix4t::Identity();
        base.block<1, 3>(0, 0) = forward;
        base.block<1, 3>(1, 0) = right;
        base.block<1, 3>(2, 0) = down;
        new_pose->t_wp = Sophus::SE3t(base);
      }
      // Set the initial velocity and bias. The initial pose is initialized to
      // align the gravity plane
      new_pose->v_w.setZero();
      new_pose->b.setZero();
    }

    {
      std::unique_lock<std::mutex>(aac_mutex);
      new_pose->cam_params = rig.cameras_[0]->GetParams();
      poses.push_back(new_pose);
    }
    axes.push_back(std::unique_ptr<SceneGraph::GLAxis>(
                      new SceneGraph::GLAxis(0.2)));
    gui_vars.scene_graph.AddChild(axes.back().get());
  }

  // Set the timestamp of the latest pose to this image's timestamp.
  poses.back()->time = timestamp + imu_time_offset;

  if (((double)tracker.num_successful_tracks() / (double)num_features) > 0.3) {
    guess = prev_delta_t_ba * prev_t_ba;
  } else {
    guess = Sophus::SE3d();
  }

  if(guess.translation() == Eigen::Vector3d(0,0,0) &&
     poses.size() > 1) {
    guess.translation() = Eigen::Vector3d(0, 0, 0.001);
  }

  if (use_imu_measurements &&
      use_imu_for_guess && poses.size() >= min_poses_for_imu) {
    std::shared_ptr<sdtrack::TrackerPose> pose1 = poses[poses.size() - 2];
    std::shared_ptr<sdtrack::TrackerPose> pose2 = poses.back();
    std::vector<ba::ImuPoseT<Scalar>> imu_poses;
    ba::PoseT<Scalar> start_pose;
    start_pose.t_wp = pose1->t_wp;
    start_pose.b = pose1->b;
    start_pose.v_w = pose1->v_w;
    start_pose.time = pose1->time;
    // Integrate the measurements since the last frame.
    std::vector<ba::ImuMeasurementT<Scalar> > meas =
        imu_buffer.GetRange(pose1->time, pose2->time);
    decltype(vi_bundle_adjuster)::ImuResidual::IntegrateResidual(
          start_pose, meas, start_pose.b.head<3>(), start_pose.b.tail<3>(),
          vi_bundle_adjuster.GetImuCalibration().g_vec, imu_poses);

    if (imu_poses.size() > 1) {
      ba::ImuPoseT<Scalar>& last_pose = imu_poses.back();
      guess = last_pose.t_wp.inverse() *
          imu_poses.front().t_wp;
      pose2->t_wp = last_pose.t_wp;
      pose2->v_w = last_pose.v_w;
      poses.back()->t_wp = pose2->t_wp;
      poses.back()->v_w = pose2->v_w;
      poses.back()->b = pose2->b;
    }
  }

  std::cerr << "Guess:\n " << guess.matrix() << std::endl;

  bool tracking_failed = false;
  {
    std::lock_guard<std::mutex> lock(aac_mutex);

    tracker.AddImage(images, guess);
    tracker.EvaluateTrackResiduals(0, tracker.GetImagePyramid(),
                                   tracker.GetCurrentTracks());

    if (!is_manual_mode) {
      tracker.OptimizeTracks(-1, optimize_landmarks, optimize_pose);
    }
    tracker.PruneTracks();

    if ((tracker.num_successful_tracks() < 10) &&
        has_imu && use_imu_measurements) {
      std::cerr << "Tracking failed. using guess." << std::endl;
      tracking_failed = true;
      tracker.set_t_ba(guess);
    }

    // Update the pose t_ab based on the result from the tracker.
    UpdateCurrentPose();
    if (follow_camera) {
      FollowCamera(gui_vars, poses.back()->t_wp);
    }
  }

  if (do_keyframing) {
    const double track_ratio = (double)tracker.num_successful_tracks() /
        (double)keyframe_tracks;
    const double total_trans = tracker.t_ba().translation().norm();
    const double total_rot = tracker.t_ba().so3().log().norm();

    bool keyframe_condition = track_ratio < 0.8 || total_trans > 0.2 ||
        total_rot > 0.1 /*|| tracker.num_successful_tracks() < 64*/;

    std::cerr << "\tRatio: " << track_ratio << " trans: " << total_trans <<
                 " rot: " << total_rot << std::endl;

    {
      std::lock_guard<std::mutex> lock(aac_mutex);
      if (keyframe_tracks != 0) {
        if (keyframe_condition) {
          is_keyframe = true;
        } else {
          is_keyframe = false;
        }
      }

      // If this is a keyframe, set it as one on the tracker.
      prev_delta_t_ba = tracker.t_ba() * prev_t_ba.inverse();

      if (is_keyframe) {
        tracker.AddKeyframe();
      }
      is_prev_keyframe = is_keyframe;
    }
  } else {
    std::lock_guard<std::mutex> lock(aac_mutex);
    tracker.AddKeyframe();
  }

  std::cerr << "Num successful : " << tracker.num_successful_tracks() <<
               " keyframe tracks: " << keyframe_tracks << std::endl;

  if (!is_manual_mode) {
    BaAndStartNewLandmarks();
  }

  if (do_self_cal || unknown_cam_calibration || unknown_imu_calibration) {
    const bool imu_plots_needed = has_imu && use_imu_measurements &&
        do_imu_self_cal;
    const uint32_t num_cam_params = rig.cameras_[0]->NumParams();
    if (candidate_window.mean.rows() == 0) {
      candidate_window.mean = rig.cameras_[0]->GetParams();
    }

    for (size_t ii = 0; ii < num_cam_params; ++ii) {
      plot_logs[ii].Log(rig.cameras_[0]->GetParams()[ii],
//         old_rig.cameras[0].camera.GenericParams()[ii],
          candidate_window.mean[ii]);
    }

    if (imu_plots_needed) {
      Eigen::Vector6d error =
          (rig.cameras_[0]->Pose().inverse() * rig.cameras_[0]->Pose()).log();
      for (size_t ii = num_cam_params; ii < num_cam_params + 6; ++ii) {
        plot_logs[ii].Log(error[ii - num_cam_params]);
      }
    }

    analysis_logs[0].Log(last_window_kl_divergence,
                         last_added_window_kl_divergence);
    analysis_logs[1].Log(tracker.num_successful_tracks());
    analysis_logs[2].Log(total_last_frame_proj_norm);
  }

  if (is_keyframe) {
    std::cerr << "KEYFRAME." << std::endl;
    keyframe_tracks = tracker.GetCurrentTracks().size();
    std::cerr << "New keyframe tracks: " << keyframe_tracks << std::endl;
  } else {
    std::cerr << "NOT KEYFRAME." << std::endl;
  }

  current_tracks = &tracker.GetCurrentTracks();

#ifdef CHECK_NANS
  _MM_SET_EXCEPTION_MASK(_MM_GET_EXCEPTION_MASK() |
                         (_MM_MASK_INVALID | _MM_MASK_OVERFLOW |
                          _MM_MASK_DIV_ZERO));
#endif

  std::cerr << "FRAME : " << frame_count << " KEYFRAME: " << poses.size() <<
               " FPS: " << frame_count / sdtrack::Toc(start_time) << std::endl;
}

void DrawImageData(uint32_t cam_id)
{
  if (cam_id == 0) {
    gui_vars.handler->track_centers.clear();
  }

  SceneGraph::AxisAlignedBoundingBox aabb;
  line_strip->Clear();
  for (uint32_t ii = 0; ii < poses.size() ; ++ii) {
    axes[ii]->SetPose(poses[ii]->t_wp.matrix());
    aabb.Insert(poses[ii]->t_wp.translation());
    Eigen::Vector3f vertex = poses[ii]->t_wp.translation().cast<float>();
    line_strip->AddVertex(vertex);
  }
  gui_vars.grid.set_bounds(aabb);

  // Draw the tracks
  for (std::shared_ptr<sdtrack::DenseTrack>& track : *current_tracks) {
    Eigen::Vector2d center;
    if (track->keypoints.back()[cam_id].tracked ||
        track->keypoints.size() <= 2 ) {
      DrawTrackData(track, image_width, image_height, center,
                    gui_vars.handler->selected_track == track, cam_id);
    }
    if (cam_id == 0) {
      gui_vars.handler->track_centers.push_back(
            std::pair<Eigen::Vector2d, std::shared_ptr<sdtrack::DenseTrack>>(
              center, track));
    }
  }

  // Populate the first column with the reference from the selected track.
  if (gui_vars.handler->selected_track != nullptr) {
    DrawTrackPatches(gui_vars.handler->selected_track, gui_vars.patches);
  }

  for (size_t cam_id = 0; cam_id < rig.cameras_.size(); ++cam_id) {
    gui_vars.camera_view[cam_id]->RenderChildren();
  }
}

void Run()
{
  std::vector<pangolin::GlTexture> gl_tex;

  // pangolin::Timer timer;
  bool capture_success = false;
  std::shared_ptr<hal::ImageArray> images = hal::ImageArray::Create();
  camera_device.Capture(*images);
  while(!pangolin::ShouldQuit()) {
    capture_success = false;
    const bool go = is_stepping;
    if (!is_running) {
      is_stepping = false;
    }
    // usleep(20000);

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor4f(1.0f,1.0f,1.0f,1.0f);

    if (go) {
      capture_success = camera_device.Capture(*images);
    }

    if (capture_success) {
      double timestamp = use_system_time ? images->Ref().system_time() :
                                           images->Ref().device_time();

      // Wait until we have enough measurements to interpolate this frame's
      // timestamp
      if (has_imu && use_imu_measurements) {
        const double start_time = sdtrack::Tic();
        while (imu_buffer.end_time < timestamp &&
               sdtrack::Toc(start_time) < 0.1) {
          usleep(10);
        }
      }

      gl_tex.resize(images->Size());

      for (int cam_id = 0 ; cam_id < images->Size() ; ++cam_id) {
        if (!gl_tex[cam_id].tid) {
          camera_img = images->at(cam_id);
          GLint internal_format = (camera_img->Format() == GL_LUMINANCE ?
                                     GL_LUMINANCE : GL_RGBA);
          // Only initialise now we know format.
          gl_tex[cam_id].Reinitialise(
                camera_img->Width(), camera_img->Height(), internal_format,
                false, 0, camera_img->Format(), camera_img->Type(), 0);
        }
      }

      camera_img = images->at(0);
      image_width = camera_img->Width();
      image_height = camera_img->Height();
      gui_vars.handler->image_height = image_height;
      gui_vars.handler->image_width = image_width;

      std::vector<cv::Mat> cvmat_images;
      for (int ii = 0; ii < images->Size() ; ++ii) {
        cvmat_images.push_back(images->at(ii)->Mat());
      }
      ProcessImage(cvmat_images, timestamp);
    }

    if (camera_img && camera_img->data()) {
      for (size_t cam_id = 0 ; cam_id < rig.cameras_.size() &&
           cam_id < (uint32_t)images->Size(); ++cam_id) {
        camera_img = images->at(cam_id);
        gui_vars.camera_view[cam_id]->ActivateAndScissor();
        gl_tex[cam_id].Upload(camera_img->data(), camera_img->Format(),
                      camera_img->Type());
        gl_tex[cam_id].RenderToViewportFlipY();
        DrawImageData(cam_id);
      }

      gui_vars.grid_view->ActivateAndScissor(gui_vars.gl_render3d);

      const ba::ImuCalibrationT<Scalar>& imu =
          vi_bundle_adjuster.GetImuCalibration();
      std::vector<ba::ImuPoseT<Scalar>> imu_poses;

      glLineWidth(1.0f);
      // Draw the inertial residual

      for (uint32_t id : ba_imu_residual_ids) {
        const ba::ImuResidualT<Scalar>& res = vi_bundle_adjuster.GetImuResidual(id);
        const ba::PoseT<Scalar>& pose = vi_bundle_adjuster.GetPose(res.pose1_id);
        std::vector<ba::ImuMeasurementT<Scalar> > meas =
            imu_buffer.GetRange(res.measurements.front().time,
                                res.measurements.back().time +
                                imu_extra_integration_time);
        res.IntegrateResidual(pose, meas, pose.b.head<3>(), pose.b.tail<3>(),
                              imu.g_vec, imu_poses);
        // std::cerr << "integrating residual with " << res.measurements.size() <<
        //              " measurements " << std::endl;
        if (pose.is_active) {
          glColor3f(1.0, 0.0, 1.0);
        } else {
          glColor3f(1.0, 0.2, 0.5);
        }

        for (size_t ii = 1 ; ii < imu_poses.size() ; ++ii) {
          ba::ImuPoseT<Scalar>& prev_imu_pose = imu_poses[ii - 1];
          ba::ImuPoseT<Scalar>& imu_pose = imu_poses[ii];
          pangolin::glDrawLine(prev_imu_pose.t_wp.translation()[0],
              prev_imu_pose.t_wp.translation()[1],
              prev_imu_pose.t_wp.translation()[2],
              imu_pose.t_wp.translation()[0],
              imu_pose.t_wp.translation()[1],
              imu_pose.t_wp.translation()[2]);
        }
      }

      if (draw_landmarks) {
        DrawLandmarks(min_lm_measurements_for_drawing, poses, rig,
                      gui_vars.handler, selected_track_id);
      }
    }
    pangolin::FinishFrame();
  }
}

void InitGui() {
  InitTrackerGui(gui_vars, window_width, window_height , image_width,
                 image_height, rig.cameras_.size());
  line_strip.reset(new SceneGraph::GLPrimitives<>);
  gui_vars.scene_graph.AddChild(line_strip.get());

  pangolin::RegisterKeyPressCallback(
        pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_RIGHT,
        [&]() {
    is_stepping = true;
  });

  pangolin::RegisterKeyPressCallback(
        pangolin::PANGO_CTRL + 's',
        [&]() {
    // write all the poses to a file.
    std::ofstream pose_file("poses.txt", std::ios_base::trunc);
    for (auto pose : poses) {
      pose_file << pose->t_wp.translation().transpose().format(
      sdtrack::kLongCsvFmt) << std::endl;
    }

    std::ofstream lm_file("landmarks.txt", std::ios_base::trunc);
    for (std::shared_ptr<sdtrack::TrackerPose> pose : poses) {
      for (std::shared_ptr<sdtrack::DenseTrack> track : pose->tracks) {
        if ((int)track->num_good_tracked_frames < min_lm_measurements_for_drawing) {
          continue;
        }
        Eigen::Vector4d ray;
        ray.head<3>() = track->ref_keypoint.ray;
        ray[3] = track->ref_keypoint.rho;
        ray = sdtrack::MultHomogeneous(pose->t_wp * rig.cameras_[0]->Pose(), ray);
        ray /= ray[3];
        lm_file << ray.transpose().format(sdtrack::kLongCsvFmt) << std::endl;
      }
    }
  });

  pangolin::RegisterKeyPressCallback('r', [&]() {
    for (uint32_t ii = 0; ii < plot_views.size(); ++ii) {
      plot_views[ii]->Keyboard(*plot_views[ii], 'a', 0, 0, true);
    }

    for (uint32_t ii = 0; ii < analysis_views.size(); ++ii) {
      analysis_views[ii]->Keyboard(*analysis_views[ii], 'a', 0, 0, true);
    }
  });

  pangolin::RegisterKeyPressCallback(' ', [&]() {
    is_running = !is_running;
  });

  pangolin::RegisterKeyPressCallback('f', [&]() {
    follow_camera = !follow_camera;
  });

  pangolin::RegisterKeyPressCallback('c', [&]() {
    do_self_cal = !do_self_cal;
  });

  pangolin::RegisterKeyPressCallback('u', [&]() {
    unknown_cam_calibration = true;
    unknown_cam_calibration_start_pose = poses.size() - 2;
    std::cerr << "Unknown calibration = true with start pose " <<
                 unknown_cam_calibration_start_pose << std::endl;
    online_calib.ClearQueue();
  });

  pangolin::RegisterKeyPressCallback('b', [&]() {
    last_optimization_level = 0;
    tracker.OptimizeTracks();
  });

  pangolin::RegisterKeyPressCallback('B', [&]() {
    do_bundle_adjustment = !do_bundle_adjustment;
    std::cerr << "Do BA:" << do_bundle_adjustment << std::endl;
  });

  pangolin::RegisterKeyPressCallback('k', [&]() {
    is_keyframe = !is_keyframe;
    std::cerr << "is_keyframe:" << is_keyframe << std::endl;
  });

  pangolin::RegisterKeyPressCallback('S', [&]() {
    do_start_new_landmarks = !do_start_new_landmarks;
    std::cerr << "Do SNL:" << do_start_new_landmarks << std::endl;
  });

  pangolin::RegisterKeyPressCallback('2', [&]() {
    last_optimization_level = 2;
    tracker.OptimizeTracks(last_optimization_level, optimize_landmarks,
                           optimize_pose);
    UpdateCurrentPose();
  });

  pangolin::RegisterKeyPressCallback('3', [&]() {
    last_optimization_level = 3;
    tracker.OptimizeTracks(last_optimization_level, optimize_landmarks,
                           optimize_pose);
    UpdateCurrentPose();
  });

  pangolin::RegisterKeyPressCallback('1', [&]() {
    last_optimization_level = 1;
    tracker.OptimizeTracks(last_optimization_level, optimize_landmarks,
                           optimize_pose);
    UpdateCurrentPose();
  });

  pangolin::RegisterKeyPressCallback('0', [&]() {
    last_optimization_level = 0;
    tracker.OptimizeTracks(last_optimization_level, optimize_landmarks,
                           optimize_pose);
    UpdateCurrentPose();
  });

  pangolin::RegisterKeyPressCallback('9', [&]() {
    last_optimization_level = 0;
    tracker.OptimizeTracks(-1, optimize_landmarks,
                           optimize_pose);
    UpdateCurrentPose();
  });

  pangolin::RegisterKeyPressCallback('p', [&]() {
    tracker.PruneTracks();
    // Update the pose t_ab based on the result from the tracker.
    UpdateCurrentPose();
    BaAndStartNewLandmarks();
  });

  pangolin::RegisterKeyPressCallback('l', [&]() {
    optimize_landmarks = !optimize_landmarks;
    std::cerr << "optimize landmarks: " << optimize_landmarks << std::endl;
  });

  pangolin::RegisterKeyPressCallback('c', [&]() {
    optimize_pose = !optimize_pose;
    std::cerr << "optimize pose: " << optimize_pose << std::endl;
  });

  pangolin::RegisterKeyPressCallback('m', [&]() {
    is_manual_mode = !is_manual_mode;
    std::cerr << "Manual mode:" << is_manual_mode << std::endl;
  });

  // set up the plotters.
  if (do_self_cal) {
    params_plot_view = &pangolin::Display("plot").SetLayout(
          pangolin::LayoutEqualVertical);
    pangolin::Display("multi").AddDisplay(*params_plot_view);

    const bool imu_plots_needed = has_imu && use_imu_measurements &&
        do_imu_self_cal;
    const uint32_t num_cam_params = rig.cameras_[0]->NumParams();
    const uint32_t num_plots = num_cam_params + (imu_plots_needed ? 6 : 0);

    plot_views.resize(num_plots);
    plot_logs.resize(num_plots);

    plot_logs[0].SetLabels({"fx - p.q.", "fx - candidate seg."});
    plot_logs[1].SetLabels({"fy - p.q.", "fy - candidate seg."});
    plot_logs[2].SetLabels({"cx - p.q.", "cx - candidate seg."});
    plot_logs[3].SetLabels({"cy - p.q.", "cy - candidate seg."});
    plot_logs[4].SetLabels({"w - p.q.", "w - candidate seg."});

    for (size_t ii = 0; ii < num_cam_params; ++ii) {
      plot_views[ii] = new pangolin::Plotter(&plot_logs[ii]);
      params_plot_view->AddDisplay(*plot_views[ii]);
      double param = rig.cameras_[0]->GetParams()[ii];
      pangolin::XYRange range(0, 500, param - param * 0.5,
                              param + param * 0.5);
      plot_views[ii]->SetDefaultView(range);
      plot_views[ii]->SetViewSmooth(range);
      plot_views[ii]->ToggleTracking();
    }

    // Add the t_vs displays.
    if (imu_plots_needed) {
      imu_plot_view = &pangolin::Display("imu_plot").SetLayout(
            pangolin::LayoutEqualVertical);
      pangolin::Display("multi").AddDisplay(*imu_plot_view);

      for (size_t ii = num_cam_params; ii < 6 + num_cam_params; ++ii) {
        plot_views[ii] = new pangolin::Plotter(&plot_logs[ii]);
        imu_plot_view->AddDisplay(*plot_views[ii]);
        pangolin::XYRange range(0, 500, -0.5, 0.5);
        plot_views[ii]->SetDefaultView(range);
        plot_views[ii]->SetViewSmooth(range);
        plot_views[ii]->ToggleTracking();
      }
    }

    analysis_plot_view = &pangolin::Display("analysis_plot").SetLayout(
          pangolin::LayoutEqualVertical);
    pangolin::Display("multi").AddDisplay(*analysis_plot_view);

    analysis_views.resize(3);
    analysis_logs.resize(3);

    analysis_logs[0].SetLabels({"p-value (candidate seg.)",
                                "p-value (last p.q. window)"});
    analysis_logs[1].SetLabels({"num. successful tracks"});
    analysis_logs[2].SetLabels({"last frame mean reproj. error"});

    for (size_t ii = 0; ii < analysis_views.size(); ++ii) {
      analysis_views[ii] = new pangolin::Plotter(&analysis_logs[ii]);
      analysis_plot_view->AddDisplay(*analysis_views[ii]);
      analysis_views[ii]->ToggleTracking();
    }
  }
}

bool LoadCameras(GetPot& cl)
{
  LoadCameraAndRig(cl, camera_device, rig);

  for (uint32_t cam_id = 0; cam_id < rig.cameras_.size(); ++cam_id) {
    selfcal_rig.AddCamera(rig.cameras_[cam_id]);
    aac_rig.AddCamera(rig.cameras_[cam_id]);
  }

  // Load the imu
  std::string imu_str = cl.follow("","-imu");
  if (!imu_str.empty()) {
    try {
      imu_device = hal::IMU(imu_str);
    } catch (hal::DeviceException& e) {
      LOG(ERROR) << "Error loading imu device: " << e.what()
                 << " ... proceeding without.";
    }
    has_imu = true;
    imu_device.RegisterIMUDataCallback(&ImuCallback);
  }

  // If we require self-calibration from an unknown initial calibration, then
  // perturb the values (camera calibraiton parameters only)
  if (unknown_cam_calibration) {
    Eigen::VectorXd params = rig.cameras_[0]->GetParams();
    // fov in rads.
    const double fov_rads = 90 * M_PI / 180.0;
    const double f_x =
        0.5 * rig.cameras_[0]->Height() / tan(fov_rads / 2);
    std::cerr << "Changing fx from " << params[0] << " to " << f_x << std::endl;
    std::cerr << "Changing fy from " << params[1] << " to " << f_x << std::endl;
    params[0] = f_x;
    params[1] = f_x;
    params[2] = rig.cameras_[0]->Width() / 2;
    params[3] = rig.cameras_[0]->Height() / 2;
    if (params.rows() > 4) {
      params[4] = 1.0;
    }

    rig.cameras_[0]->SetParams(params);
    selfcal_rig.cameras_[0]->SetParams(params);
    aac_rig.cameras_[0]->SetParams(params);
    // Add a marker in the batch file for this initial, unknown calibration.
    Eigen::VectorXd initial_covariance(params.rows());
    initial_covariance.setOnes();
    std::ofstream("batch.txt", std::ios_base::app) << 0 << ", " <<
      initial_covariance.transpose().format(sdtrack::kLongCsvFmt) <<
      ", " << 0 << ", " << params.transpose().format(sdtrack::kLongCsvFmt) <<
      std::endl;
  }

  if (has_imu && unknown_imu_calibration) {

    rig.cameras_[0]->Pose().so3() = rig.cameras_[0]->Pose().so3() *
        Sophus::SO3d::exp(
          (Eigen::Vector3d() << 0.1, 0.2, 0.3).finished());

    for (uint32_t cam_id = 0; cam_id < rig.cameras_.size(); ++cam_id) {
      selfcal_rig.cameras_[cam_id]->Pose() = rig.cameras_[cam_id]->Pose();
      aac_rig.cameras_[cam_id]->Pose() = aac_rig.cameras_[cam_id]->Pose();
    }

  }

  if (has_imu && use_imu_measurements) {
    // Capture an image so we have some IMU data.
    std::shared_ptr<hal::ImageArray> images = hal::ImageArray::Create();
    while (imu_buffer.elements.size() == 0) {
      camera_device.Capture(*images);
    }
  }

  return true;
}

int main(int argc, char** argv) {
  // Clear the log files.
  {
    std::ofstream sigmas_file("sigmas.txt", std::ios_base::trunc);
    std::ofstream pq_file("pq.txt", std::ios_base::trunc);
    std::ofstream batch_file("batch.txt", std::ios_base::trunc);
    std::ofstream pose_file("timings.txt", std::ios_base::trunc);
  }
  srand(0);
  google::InitGoogleLogging(argv[0]);

  GetPot cl(argc, argv);
  if (cl.search("--help")) {
    LOG(INFO) << g_usage;
    exit(-1);
  }

  if (cl.search("-use_system_time")) {
    use_system_time = true;
  }

  if (cl.search("-startnow")) {
    is_running = true;
  }

  LOG(INFO) << "Initializing camera...";
  LoadCameras(cl);

  pyramid_levels = 3;
  patch_size = 7;
  sdtrack::KeypointOptions keypoint_options;
  keypoint_options.gftt_feature_block_size = patch_size;
  keypoint_options.max_num_features = num_features * 2;
  keypoint_options.gftt_min_distance_between_features = 3;
  keypoint_options.gftt_absolute_strength_threshold = 0.005;
  sdtrack::TrackerOptions tracker_options;
  tracker_options.pyramid_levels = pyramid_levels;
  tracker_options.detector_type = sdtrack::TrackerOptions::Detector_GFTT;
  tracker_options.num_active_tracks = num_features;
  tracker_options.use_robust_norm_ = false;
  tracker_options.robust_norm_threshold_ = 30;
  tracker_options.patch_dim = patch_size;
  tracker_options.default_rho = 1.0/5.0;
  tracker_options.feature_cells = feature_cells;
  tracker_options.iteration_exponent = 2;
  tracker_options.dense_ncc_threshold = ncc_threshold;
  tracker_options.harris_score_threshold = 2e6;
  tracker_options.gn_scaling = 1.0;
  tracker.Initialize(keypoint_options, tracker_options, &rig);

  // Initialize the online calibration component.
  Eigen::VectorXd weights(rig.cameras_[0]->NumParams());
  if (weights.rows() > 4) {
    weights << 1.0, 1.0, 1.7, 1.7, 320000;
    //weights << 1.0, 1.0, 1.0, 1.0, 1.0;
  } else {
    weights << 1.0, 1.0, 1.7, 1.7;
  }

  /// ZZZZZZZ : TEMPORARY FOR IMU
  // weights = Eigen::VectorXd(6);
  // weights << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

  online_calib.Init(&aac_mutex, &selfcal_rig, num_self_cal_segments,
                    self_cal_segment_length, weights,
                    imu_time_offset, &imu_buffer);

  InitGui();

  //////////////////////////
  /// ZZZZZZZZZZZZZZZZZZ: Get rid of this. Only valid for ICRA test rig
  imu_time_offset = -0.0697;

  aac_thread = std::shared_ptr<std::thread>(new std::thread(&DoAAC));

  Run();

  return 0;
}


#include "autodataassociation.h"

#include "include/core/g2o_object.h"
#include "instance.h"

#include <Eigen/Core>

#include "DataStruct.h"

#include <algorithm>

using namespace Eigen;
using namespace std;

bool compare_func(pair<int,double>& p1, pair<int,double>& p2)
{
    return p1.second < p2.second;
}

void AutoDataAssociation::initialize(vector<Instance> &instances, Matrix3d& calib)
{

    mmCalib = calib;
    initializeInstances(instances);

}

void AutoDataAssociation::initializeInstances(vector<Instance> &instances)
{
    mvpEllipsoids.clear();
    int num = instances.size();
    for(int i=0;i<num;i++){
        auto ins = instances[i];
        g2o::ellipsoid* e = generateEllipsoid(ins);
        mvpEllipsoids.push_back(e);
    }

    return;

}

// 输入: 当前帧gt,
vector<Association> AutoDataAssociation::process(VectorXd &poseTwc, MatrixXd &detMat){
    int objNum = mvpEllipsoids.size();

    vector<ProjEllipse> projEllipses;
    // 遍历 instances
    for(int i=0;i<objNum;i++)
    {
        g2o::ellipsoid* e = mvpEllipsoids[i];

        g2o::SE3Quat Twc(poseTwc.tail(7));  // xyz q4
        Vector4d bbox = e->getBoundingBoxFromProjection(Twc.inverse(), mmCalib);

        // 定义一个投影体, 方便彼此进行数据关联.
        ProjEllipse proje;
        proje.label = e->miLabel;
        proje.bbox = bbox;
        proje.id = i;

        projEllipses.push_back(proje);
    }

    // 开始做关联
    vector<Association> associations = associateProjEllipseAndDetections(projEllipses, detMat);

    return associations;

}

g2o::ellipsoid* AutoDataAssociation::generateEllipsoid(Instance& ins){
    g2o::ellipsoid* e = new g2o::ellipsoid;
    Eigen::Matrix<double, 9, 1> v;
    for(int n=0;n<9;n++)
        v(n) = ins.param[n];
    e->fromMinimalVector(v);

    // 设置 label id.
    e->miLabel = ins.label;

    return e;
}

vector<Association> AutoDataAssociation::associateProjEllipseAndDetections(vector<ProjEllipse>& projEllipses, MatrixXd &detMat)
{
    double THRESH_DIS = 150; // 中心 x y差距和不超过多少像素
    // -------
    vector<Association> associations;
    vector<ProjEllipse> projEllipsesSelected; // 存储那些被选召的椭圆

    // 针对每个ob det 从候选椭球挑一个
    //  -> 依据: label一致(?错误检测, 很少的情况不考虑), 挑选距离最近, 而且距离满足阈值.
    int detNum = detMat.rows();

    vector<ProjEllipse> projEllipsesLeft = projEllipses;
    int objNum = projEllipsesLeft.size();
    vector<bool> projsChosen(objNum, false);
    for( int i=0; i< detNum; i++ )
    {
        Association asso;

        VectorXd det = detMat.row(i);
        // 挑选出所有label一致的.
        int label = det(DET_TABLE_LABEL);
        double x1 = det(DET_TABLE_X1);
        double y1 = det(DET_TABLE_Y1);
        double x2 = det(DET_TABLE_X2);
        double y2 = det(DET_TABLE_Y2);

        if(objNum > 0){
            vector<pair<int,double>> associationDistances;
            for(int n=0;n<objNum;n++)
            {
                if(projsChosen[n]) continue;        // 已经选中的不再考虑
                ProjEllipse proj = projEllipsesLeft[n];
                // if(proj.label!=label) continue;

                // 计算矩形框中间差值
                double dis = std::abs(x1+x2-proj.bbox(0)-proj.bbox(2))/2
                        + std::abs(y1+y2-proj.bbox(1)-proj.bbox(3))/2;
                associationDistances.push_back(make_pair(n, dis));
            }

            if(associationDistances.size() > 0){
                // 排序取最小的一个.
                sort(associationDistances.begin(), associationDistances.end(), compare_func);

                double mini_dis = associationDistances[0].second;
                int best_instance_id = associationDistances[0].first;
                if(mini_dis < THRESH_DIS )
                {
                    // ok
                    Association asso;
                    asso.instanceID = best_instance_id;
                    asso.detID = i;
                    asso.prob = 1;

                    associations.push_back(asso);

                    projsChosen[best_instance_id] = true;
                    projEllipsesSelected.push_back(projEllipses[best_instance_id]);

                }
            }
        }

    }
    // 依次循环得到关联结果.
    mvProjEllipses.clear();
    mvProjEllipses = projEllipsesSelected;

    return associations;
}

cv::Mat AutoDataAssociation::drawProjection(cv::Mat &in)
{
    cv::Mat out = in.clone();
    int num = mvProjEllipses.size();
    for(auto proj : mvProjEllipses )
    {
        Vector4d rect = proj.bbox;
        cv::rectangle(out, cv::Rect(cv::Point(rect[0],rect[1]),cv::Point(rect[2],rect[3])), cv::Scalar(0,0,255), 2);
    }

    return out.clone();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ATAND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATAND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing atand capabilities

    inverse tangent in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = atand(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[-90, 90[\f$ such that <tt>tand(r) == x</tt>.

    @see atan2d, atan2, atan, atanpi

  **/
  const boost::dispatch::functor<tag::atand_> atand = {};
} }
#endif

#include <boost/simd/function/scalar/atand.hpp>
#include <boost/simd/function/simd/atand.hpp>

#endif


#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/structured_light.hpp>
#include <opencv2/opencv_modules.hpp>
#include <Windows.h>
#include <Eigen/dense>
#include <Eigen/Geometry>
#include <opencv2/surface_matching.hpp>
#include <opencv2/surface_matching/ppf_helpers.hpp>

// (if you did not build the opencv_viz module, you will only see the disparity images)
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif

using namespace std;
using namespace cv;

static const char* keys =
{ "{@images_list | | Image list where the captured pattern images are saved}"
	"{@calib_param_path     | | Calibration_parameters            }"
	"{@proj_width      | | The projector width used to acquire the pattern          }"
	"{@proj_height     | | The projector height used to acquire the pattern}"
	"{@white_thresh     | | The white threshold height (optional)}"
	"{@black_thresh     | | The black threshold (optional)}" };

static void help()
{
	cout << "\nThis example shows how to use the \"Structured Light module\" to decode a previously acquired gray code pattern, generating a pointcloud"
		"\nCall:\n"
		"./example_structured_light_pointcloud <images_list> <calib_param_path> <proj_width> <proj_height> <white_thresh> <black_thresh>\n"
		<< endl;
}

//Mat plane_from_points(Mat display)
//{
//	vector<Vec3f> c;
//	for (size_t i = 0; i < display.cols; i++)
//	{
//		for (size_t j = 0; j < display.rows; j++)
//		{
//			c.push_back(Vec3f(i, j, display.at<float>(i, j)));
//		}
//	}
//
//	// copy coordinates to  matrix in Eigen format
//	size_t num_atoms = c.size();
//	Eigen::Matrix< Vec3f::, Eigen::Dynamic, Eigen::Dynamic> coord(3, num_atoms);
//	for (size_t i = 0; i < num_atoms; ++i) coord.col(i) = c[i];
//
//	// calculate centroid
//	//Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());
//	Point3f centroid = Point3f(coord.row(0).mean(), coord.row(1).mean(), coord.row(2).mean());
//
//	// subtract centroid
//	coord.row(0).array() -= centroid(0);
//	coord.row(1).array() -= centroid(1);
//	coord.row(2).array() -= centroid(2);
//
//	// we only need the left-singular matrix here
//
//	auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
//	Point3f plane_normal = svd.matrixU().rightCols<1>();
//
//	float x = plane_normal[0];
//	float y = plane_normal[1];
//	float z = plane_normal[2];
//
//
//	float angle = atan2(x, z) * 180 / CV_PI;
//	float angle2 = atan2(y, z) * 180 / CV_PI;
//
//	Mat plane = Mat(angle, angle2, CV_32F);
//
//	return plane;
//}

Mat PlaneFitting(Mat disparity)
{
	Vec3f sum;
	float xx, xy, yy, n;
	n = disparity.cols * disparity.rows;
	for (size_t i = 0; i < disparity.cols; i++)
	{
		for (size_t j = 0; j < disparity.rows; j++)
		{

		}
	}

}

int main(int argc, char** argv)
{
	structured_light::GrayCodePattern::Params params;
	CommandLineParser parser(argc, argv, keys);

	params.width = 1024;
	params.height = 768;

	// Set up GraycodePattern with params
	Ptr<structured_light::GrayCodePattern> graycode = structured_light::GrayCodePattern::create(params);
	size_t white_thresh = 0;
	size_t black_thresh = 0;

	if (argc == 2)
	{
		// If passed, setting the white and black threshold, otherwise using default values
		white_thresh = parser.get<unsigned>(4);
		black_thresh = parser.get<unsigned>(5);

		graycode->setWhiteThreshold(white_thresh);
		graycode->setBlackThreshold(black_thresh);
	}
	String imagelist = "E:\\Computer Vision\\19gray\\cam\\cam (";
	string patternlist = "E:\\Computer Vision\\19gray\\pattern\\pattern_im";

	// Loading calibration parameters

	size_t numberOfPatternImages = graycode->getNumberOfPatternImages();
	vector<vector<Mat> > captured_pattern;
	captured_pattern.resize(2);
	captured_pattern[0].resize(numberOfPatternImages);
	captured_pattern[1].resize(numberOfPatternImages);

	Mat color = imread(imagelist + to_string(numberOfPatternImages + 1) + ").bmp", IMREAD_COLOR);
	Size imagesSize = color.size();

	// Stereo rectify

	// Loading pattern images
	for (size_t i = 0; i < numberOfPatternImages; i++)
	{
		captured_pattern[0][i] = imread(imagelist + to_string(i + 1) + ").bmp", IMREAD_GRAYSCALE);
		captured_pattern[1][i] = imread(patternlist + to_string(i + 1) + ".png", IMREAD_GRAYSCALE);

		resize(captured_pattern[0][i], captured_pattern[0][i], Size(1024, 768));
		resize(captured_pattern[1][i], captured_pattern[1][i], Size(1024, 768));

		if ((!captured_pattern[0][i].data) || (!captured_pattern[1][i].data))
		{
			cout << "Empty images" << endl;
			help();
			return -1;
		}
	}
	cout << "done" << endl;

	vector<Mat> blackImages;
	vector<Mat> whiteImages;

	blackImages.resize(2);
	whiteImages.resize(2);

	// Loading images (all white + all black) needed for shadows computation
	cvtColor(color, whiteImages[0], COLOR_RGB2GRAY);

	whiteImages[1] = imread(patternlist + "41.png", IMREAD_GRAYSCALE);
	blackImages[0] = imread(imagelist + "42).bmp", IMREAD_GRAYSCALE);
	blackImages[1] = imread(patternlist + "42.png", IMREAD_GRAYSCALE);

	resize(whiteImages[0], whiteImages[0], Size(1024, 768));
	resize(whiteImages[1], whiteImages[1], Size(1024, 768));
	resize(blackImages[0], blackImages[0], Size(1024, 768));
	resize(blackImages[1], blackImages[1], Size(1024, 768));

	cout << endl << "Decoding pattern ..." << endl;
	Mat disparityMap;
	bool decoded = graycode->decode(captured_pattern, disparityMap, blackImages, whiteImages, structured_light::DECODE_3D_UNDERWORLD);

	if (decoded)
	{
		cout << endl << "pattern decoded" << endl;
		imwrite("E:\\Computer Vision\\19gray\\decode\\disparityMap.png", disparityMap);
		// To better visualize the result, apply a colormap to the computed disparity
		double min;
		double max;
		minMaxIdx(disparityMap, &min, &max);
		Mat cm_disp, scaledDisparityMap, DS, FDS;
		cout << "disp min " << min << endl << "disp max " << max << endl;
		convertScaleAbs(disparityMap, scaledDisparityMap, 255 / (max - min));
		applyColorMap(scaledDisparityMap, cm_disp, COLORMAP_TWILIGHT);
		imwrite("E:\\Computer Vision\\19gray\\decode\\scaledDisparityMap.png", scaledDisparityMap);

		// Show the result
		//resize(cm_disp, cm_disp, Size(640, 480), 0, 0, INTER_LINEAR_EXACT);
		imshow("cm disparity m", cm_disp);
		imwrite("E:\\Computer Vision\\19gray\\decode\\cm_disp.png", cm_disp);

		// Compute the point cloud
		//disparityMap.convertTo(disparityMap, CV_32FC1);
		imwrite("E:\\Computer Vision\\19gray\\decode\\convertToCV_32FC1disp.png", disparityMap);
		// Compute a mask to remove background
		Mat dst, thresholded_disp;
		threshold(scaledDisparityMap, thresholded_disp, 0, 255, THRESH_OTSU + THRESH_BINARY);
		//resize(thresholded_disp, dst, Size(640, 480), 0, 0, INTER_LINEAR_EXACT);
		imshow("threshold disp otsu", thresholded_disp);
		imwrite("E:\\Computer Vision\\19gray\\decode\\thresholded_disp.png", thresholded_disp);

		///
		Beep(1568, 200);
		Beep(1568, 200);
		Beep(1568, 200);
		Beep(1245, 1000);
		Beep(1397, 200);
		Beep(1397, 200);
		Beep(1397, 200);
		Beep(1175, 1000);
		///

		//disparityMap.p

#if 0//def HAVE_OPENCV_VIZ
		// Apply the mask to the point cloud
		Mat pointcloud_tresh, color_tresh;
		color.copyTo(color_tresh, thresholded_disp);

		// Show the point cloud on viz
		viz::Viz3d myWindow("Point cloud with color");
		myWindow.setBackgroundMeshLab();
		myWindow.showWidget("coosys", viz::WCoordinateSystem());
		myWindow.showWidget("pointcloud", viz::WCloud(pointcloud_tresh, color_tresh));
		myWindow.showWidget("text2d", viz::WText("Point cloud", Point(20, 20), 20, viz::Color::green()));
		myWindow.spin();
#endif // HAVE_OPENCV_VIZ

	}

	waitKey();
	return 0;
}


/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2017 by Florian Beck <florian.beck@tuwien.ac.at>        *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder nor the names of its      *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS   *
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT     *
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS     *
 *   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE        *
 *   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,  *
 *   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,  *
 *   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER      *
 *   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT    *
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
 *   WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           *
 *   POSSIBILITY OF SUCH DAMAGE.                                           *
 ***************************************************************************/

#include "heatmap_system_model.h"
#include <assert.h>
#include <common.h>
#include <boost/math/distributions/normal.hpp>

HeatMapSystemModel::HeatMapSystemModel(double sigma_x, double sigma_y, double sigma_theta, int angle_partitions, double gamma, grid_map::GridMap& heat_map,
                                       grid_map::Matrix* layer) : SystemModel(4)
{
  std::random_device rd;
  generator_ = std::mt19937(rd());
  normal_distribution_ = std::normal_distribution<double>();
  sigma_x_ = sigma_x;
  sigma_y_ = sigma_y;
  sigma_theta_ = sigma_theta;
  heat_map_ = heat_map;
  layer_ = layer;
  angle_partitions_ = angle_partitions;
//  direction_probabilities_.resize(angle_partitions);
  direction_probabilities_.assign(angle_partitions, 1.0);
  weight_factor_.resize(angle_partitions);
  gamma_ = gamma;

  sum_for_weight_factor_ = 0;

  // precompute weight factors
  for(int i = 0; i < angle_partitions_; i++)
  {
    double angle = atan2(sin(i * 2 * M_PI / angle_partitions_), cos(i * 2 * M_PI / angle_partitions_));

    boost::math::normal norm = boost::math::normal(0, sigma_theta_ * sigma_theta_);
    weight_factor_.at(i) = boost::math::pdf(norm, angle);
  }
}

void HeatMapSystemModel::sample(Eigen::Ref<Eigen::VectorXd> state, double dt, const Eigen::Ref<const Eigen::VectorXd>& meas = Eigen::VectorXd(), const Eigen::Ref<const Eigen::MatrixXd>& meas_cov = Eigen::MatrixXd())
{
  // check param dimensions
  assert(state.size() >= state_size_);
  Eigen::VectorXd sample = state;

  // current state vel vector angle theta and magnitude r
  double theta = atan2(state(static_cast<int>(State::VY)), state(static_cast<int>(State::VX)));
  double r = sqrt(state(static_cast<int>(State::VX)) * state(static_cast<int>(State::VX)) + state(static_cast<int>(State::VY)) * state(static_cast<int>(State::VY)));

  // calculate map index from current position
  grid_map::Index idx;
  grid_map::Position pos(state(static_cast<int>(State::X)), state(static_cast<int>(State::Y)));
  double theta_tar;
  int direction;

  direction_probabilities_.assign(angle_partitions_, 1.0);

  if (heat_map_.getIndex(pos, idx))
  {
    bool all_zero = true;

    for(int i = 0; i < angle_partitions_; i++)
    {
      theta_tar = theta + i * 2 * M_PI / angle_partitions_;

      grid_map::Index next_idx;
      grid_map::Position next_pos = pos + grid_map::Position(r * cos(theta_tar), r * sin(theta_tar));

      // line iterator from idx to next_idx
      // multiply values

      if(heat_map_.getIndex(next_pos, next_idx))
      {
        unsigned int it_steps = 0;
        for(grid_map::LineIterator it(heat_map_, idx, next_idx); !it.isPastEnd(); ++it)
        {
          const grid_map::Index current_idx(*it);
          const size_t lin_idx = grid_map::getLinearIndexFromIndex(current_idx, heat_map_.getSize());

          direction_probabilities_[i] *= (*layer_)(lin_idx);
          //std::cout << "dir prob(" << i << ") = " << direction_probabilities_[i] << std::endl;

          it_steps++;
        }
        // normalize bc of line iterator steps
        // also dep. on velocity length might be different
        direction_probabilities_[i] /= static_cast<double>(it_steps);
        direction_probabilities_[i] *= weight_factor_[i];

        if(direction_probabilities_.back() != 0.0)
        {
          all_zero = false;
        }
      }
      /*
      if(heat_map_.getIndex(next_pos, next_idx))
      {
        const size_t lin_idx = grid_map::getLinearIndexFromIndex(next_idx, heat_map_.getSize());

        direction_probabilities_.at(i) = (*layer_)(lin_idx) * weight_factor_.at(i);

        if(direction_probabilities_.back() != 0.0)
        {
          all_zero = false;
        }

      }*/
      else
      {
        direction_probabilities_[i] = 0.0;
      }
    }

    if (!all_zero)
    {
      std::discrete_distribution<int> direction_distribution(direction_probabilities_.begin(),
                                                             direction_probabilities_.end());
      direction = direction_distribution(generator_);
    }
    else
    {
      direction = 0;
    }
  }
  else
  {
    direction = 0;
  }

  double theta_diff = direction * 2 * M_PI / angle_partitions_;

  double theta_next = theta + atan2(sin(theta_diff), cos(theta_diff)) * gamma_;

  sample(static_cast<int>(State::X)) = state(static_cast<int>(State::X)) + state(static_cast<int>(State::VX)) * dt + dt * dt * 0.5 * sigma_x_ * sigma_x_ * normal_distribution_(generator_);
  sample(static_cast<int>(State::Y)) = state(static_cast<int>(State::Y)) + state(static_cast<int>(State::VY)) * dt + dt * dt * 0.5 * sigma_y_ * sigma_y_ * normal_distribution_(generator_);

  sample(static_cast<int>(State::VX)) = r * cos(theta_next) + dt * sigma_x_ * sigma_x_ * normal_distribution_(generator_);
  sample(static_cast<int>(State::VY)) = r * sin(theta_next) + dt * sigma_y_ * sigma_y_ * normal_distribution_(generator_);

  state = sample;
}


#include <jni.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <Eigen/Geometry>

#include "point_cloud/include/PointCloudTransform.h"

extern "C"
{
	JNIEXPORT void JNICALL Java_it_unibo_slam_gui_opengl_model_PointCloudStatic_reprojectPointCloudVertices(JNIEnv *env, jobject obj,
			jfloatArray transform, jint pointArrayLength, jfloatArray pointsInOut);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_gui_opengl_model_PointCloudStatic_reprojectPointCloudVertices(JNIEnv *env, jobject obj,
		jfloatArray transform, jint pointArrayLength, jfloatArray pointsInOut)
{
	float tempTransform[16];
	env->GetFloatArrayRegion(transform, 0, 16, tempTransform);
	Eigen::Matrix4f transformEigen(Eigen::Matrix4f::Map(tempTransform));

	float *tempPointsInOut = env->GetFloatArrayElements(pointsInOut, NULL);

	reprojectPointCloud(tempPointsInOut, tempPointsInOut, pointArrayLength, transformEigen);

	env->ReleaseFloatArrayElements(pointsInOut, tempPointsInOut, 0);
}


#include <mass.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(box)
{
  double const rho = 1.0;
  double const W   = 1.0;
  double const H   = 1.0;
  double const D   = 1.0;

  mass::Properties<double> P = mass::compute_box(rho,W,H,D);

  BOOST_CHECK_CLOSE( P.m_m, 8.0, 0.01 );
  BOOST_CHECK( P.m_Ixx > 0.0 );
  BOOST_CHECK( P.m_Iyy > 0.0 );
  BOOST_CHECK( P.m_Izz > 0.0 );

  BOOST_CHECK_CLOSE( P.m_Ixx, P.m_Iyy, 0.01 );
  BOOST_CHECK_CLOSE( P.m_Iyy, P.m_Izz, 0.01 );

  BOOST_CHECK( !P.is_model_space() );
  BOOST_CHECK(  P.is_body_space()  );
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2016 Maikel Nadolski
 *
 * 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.
 */

#include <fstream>
#include <iostream>

#include <boost/iterator/function_input_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include "maikel/hmm/hidden_markov_model.h"
#include "maikel/hmm/sequence_generator.h"
#include "maikel/hmm/io.h"

enum Exit_Error_Codes {
  exit_success = 0,
  exit_not_enough_arguments = 1,
  exit_io_error = 2,
  exit_argument_error = 3
};

int main(int argc, char *argv[])
{
  if (argc < 3) {
    std::cerr << "Usage: " << argv[0] << " <model.dat> <sequence-length>\n";
    return exit_not_enough_arguments;
  }
  std::ifstream input(argv[1]);
  input.exceptions(std::ifstream::failbit | std::ifstream::badbit);
  auto model = maikel::hmm::read_hidden_markov_model<float>(input);
  input.close();

  // Try to read the HMM-model that will be used to generate a random sequence.

  std::istringstream obslen_converter(argv[2]);
  std::size_t obslen;
  if (!(obslen_converter >> obslen) || obslen == 0) {
    std::cerr << "Could not convert sequence length to std::size_t.\n";
    return exit_argument_error;
  }

  using Index = decltype(model)::size_type;
  std::copy(boost::make_counting_iterator<Index>(0),
            boost::make_counting_iterator<Index>(model.symbols()),
            std::ostream_iterator<Index>(std::cout, " "));
  std::cout << "\n" << obslen << "\n";
  std::function<Index()> generator = maikel::hmm::make_sequence_generator(model);
  std::copy(boost::make_function_input_iterator(generator, std::size_t{0}),
            boost::make_function_input_iterator(generator, obslen),
            std::ostream_iterator<Index>(std::cout, " "));
  std::cout << std::endl;

  return exit_success;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench 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.
// __END_LICENSE__

// TestCorrelator.h
#include <test/Helpers.h>

#include <vw/Image.h>
#include <vw/Stereo/SubpixelView.h>
#include <vw/FileIO.h>
#include <boost/foreach.hpp>
#include <boost/random/linear_congruential.hpp>

using namespace vw;
using namespace vw::stereo;

namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

template <int32 istretch>
class SubPixelCorrelateTest : public ::testing::Test {
  const int32 IMAGE_SIZE, HALF_IMAGE_SIZE;

public:
  SubPixelCorrelateTest() : IMAGE_SIZE(100), HALF_IMAGE_SIZE(50) {}

protected:
  typedef LaplacianOfGaussian PreFilter;

  void SetUp() {
    stretch = float32(istretch)/100;

    boost::rand48 gen(10);
    image1 = transform(channel_cast_rescale<uint8>(uniform_noise_view( gen, IMAGE_SIZE, IMAGE_SIZE )),
                       AffineTransform(Matrix2x2(3,0,0,3),Vector2()),
                       ZeroEdgeExtension(), BicubicInterpolation());
    translation = HALF_IMAGE_SIZE-HALF_IMAGE_SIZE*stretch;
    image2 = transform(image1, AffineTransform(Matrix2x2(stretch,0,0,1),
                                               Vector2(translation,0) ),
                       ZeroEdgeExtension(), BicubicInterpolation());

    starting_disp.set_size(IMAGE_SIZE,IMAGE_SIZE);
    for ( int32 i = 0; i < IMAGE_SIZE ; i++ ) {
      int32 disparity =
        boost::numeric_cast<int32>(stretch * i + translation - i);
      for ( int32 j = 0; j < IMAGE_SIZE; j++ )
        starting_disp(i,j) = disparity;
    }
  }

  template <class ViewT>
  double check_error( ImageViewBase<ViewT> const& input,
                      int32& invalid_count ) {
    ViewT const& disparity = input.impl();
    double error = 0;
    for ( int32 i = 0; i < IMAGE_SIZE; i++ ) {
      float expected = stretch * float(i) + translation - i;
      for ( int32 j = 0; j < IMAGE_SIZE; j++ ) {
        error += disparity(i,j)[1] + fabs(disparity(i,j)[0] - expected);
        if ( !is_valid(disparity(i,j)) )
          invalid_count++;
      }
    }
    return error / (double(IMAGE_SIZE)*double(IMAGE_SIZE));
  }

  float32 stretch, translation;
  ImageView<uint8> image1, image2;
  ImageView<PixelMask<Vector2f> > starting_disp;
};

typedef SubPixelCorrelateTest<95> SubPixelCorrelate95Test;
typedef SubPixelCorrelateTest<90> SubPixelCorrelate90Test;
typedef SubPixelCorrelateTest<80> SubPixelCorrelate80Test;
typedef SubPixelCorrelateTest<70> SubPixelCorrelate70Test;

// Testing Parabola Subpixel View
//--------------------------------------------------------------
TEST( ParabolaSubpixel, NullTest ) {
  ImageView<PixelMask<Vector2i> > disparity(5,5);
  fill( disparity, PixelMask<Vector2i>(Vector2i(1,1)) );
  ImageView<float> left(5,5), right(5,5);
  fill( left, 0.5 );
  fill( right, 0.6 );

  ImageView<PixelMask<Vector2f> > fdisparity =
    parabola_subpixel( disparity, left, right,
                       NullOperation(),
                       Vector2i(3,3) );
  EXPECT_EQ( fdisparity.cols(), 5 );
  EXPECT_EQ( fdisparity.rows(), 5 );
  BOOST_FOREACH( PixelMask<Vector2f> const& fdisp, fdisparity ) {
    EXPECT_TRUE( is_valid( fdisp ) );
    EXPECT_VECTOR_NEAR( fdisp.child(), Vector2f(1,1), 0.1 );
  }

  fdisparity =
    parabola_subpixel( disparity, left, right,
                       LaplacianOfGaussian(1.4),
                       Vector2i(3,3) );
  EXPECT_EQ( fdisparity.cols(), 5 );
  EXPECT_EQ( fdisparity.rows(), 5 );
  BOOST_FOREACH( PixelMask<Vector2f> const& fdisp, fdisparity ) {
    EXPECT_TRUE( is_valid( fdisp ) );
    EXPECT_VECTOR_NEAR( fdisp.child(), Vector2f(1,1), 0.1 );
  }
}

typedef SubPixelCorrelateTest<95> SubPixelCorrelate95Test;
typedef SubPixelCorrelateTest<90> SubPixelCorrelate90Test;
typedef SubPixelCorrelateTest<80> SubPixelCorrelate80Test;
typedef SubPixelCorrelateTest<70> SubPixelCorrelate70Test;

TEST_F( SubPixelCorrelate95Test, Parabola ) {
  ImageView<PixelMask<Vector2f> > disparity_map =
    parabola_subpixel( starting_disp, image1, image2,
                       PreFilter(1.4),
                       Vector2i(7,7) );

  int32 invalid_count = 0;
  double error = check_error( disparity_map, invalid_count );
  EXPECT_LT(error, 0.6);
  EXPECT_LE(invalid_count, 0);
}

// Testing Bayes EM SubPixel
//--------------------------------------------------------------
TEST_F( SubPixelCorrelate95Test, BayesEM95 ) {
  ImageView<PixelMask<Vector2f> > disparity_map =
    bayes_em_subpixel( starting_disp,
                       channel_cast_rescale<float>(image1),
                       channel_cast_rescale<float>(image2),
                       PreFilter(1.4),
                       Vector2i(7,7) );
  int32 invalid_count = 0;
  double error = check_error( disparity_map, invalid_count );
  //std::cout << "Err: " << error << " Cnt: " << invalid_count << "\n";
  //EXPECT_LT(error, 0.054);      // Use for subpixel w/o pyramid
  //EXPECT_LE(invalid_count, 0);
  EXPECT_LT(error, 0.35);
  EXPECT_LE(invalid_count, 12);
}

TEST_F( SubPixelCorrelate90Test, BayesEM90 ) {
  ImageView<PixelMask<Vector2f> > disparity_map =
    bayes_em_subpixel( starting_disp,
                       channel_cast_rescale<float>(image1),
                       channel_cast_rescale<float>(image2),
                       PreFilter(1.4),
                       Vector2i(7,7) );
  int32 invalid_count = 0;
  double error = check_error( disparity_map, invalid_count );
  //std::cout << "Err: " << error << " Cnt: " << invalid_count << "\n";
  //EXPECT_LT(error, 0.078);     // Use for subpixel w/o pyramid
  //EXPECT_LE(invalid_count, 3);
  EXPECT_LT(error, 0.36);
  EXPECT_LE(invalid_count, 12);
}

TEST_F( SubPixelCorrelate80Test, BayesEM80 ) {
  ImageView<PixelMask<Vector2f> > disparity_map =
    bayes_em_subpixel( starting_disp,
                       channel_cast_rescale<float>(image1),
                       channel_cast_rescale<float>(image2),
                       PreFilter(1.4),
                       Vector2i(7,7) );
  int32 invalid_count = 0;
  double error = check_error( disparity_map, invalid_count );
  //std::cout << "Err: " << error << " Cnt: " << invalid_count << "\n";
  //EXPECT_LT(error, 0.125);     // Use for subpixel w/o pyramid
  //EXPECT_LE(invalid_count, 3);
  EXPECT_LT(error, 0.52);
  EXPECT_LE(invalid_count, 22);
}

TEST_F( SubPixelCorrelate70Test, BayesEM70 ) {
  ImageView<PixelMask<Vector2f> > disparity_map =
    bayes_em_subpixel( starting_disp,
                       channel_cast_rescale<float>(image1),
                       channel_cast_rescale<float>(image2),
                       PreFilter(1.4),
                       Vector2i(7,7) );
  int32 invalid_count = 0;
  double error = check_error( disparity_map, invalid_count );
  //std::cout << "Err: " << error << " Cnt: " << invalid_count << "\n";
  //EXPECT_LT(error, 0.198);      // Use for subpixel w/o pyramid
  //EXPECT_LE(invalid_count, 7);
  EXPECT_LT(error, 0.9);
  EXPECT_LE(invalid_count, 48);
}