/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

#include <iostream>
#include <algorithm>
#include <set>
#include <Eigen/Dense>

#include <sensor_msgs/image_encodings.h>
#include <random_numbers/random_numbers.h>

#include <msckf_vio/CameraMeasurement.h>
#include <msckf_vio/TrackingInfo.h>
#include <msckf_vio/image_processor.h>
#include <msckf_vio/utils.h>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace msckf_vio {
ImageProcessor::ImageProcessor(ros::NodeHandle& n) :
  nh(n),
  is_first_img(true),
  //img_transport(n),
  stereo_sub(10),
  prev_features_ptr(new GridFeatures()),
  curr_features_ptr(new GridFeatures()) {
  return;
}

ImageProcessor::~ImageProcessor() {
  destroyAllWindows();
  //ROS_INFO("Feature lifetime statistics:");
  //featureLifetimeStatistics();
  return;
}

// 导入节点launch时提供的各种参数
bool ImageProcessor::loadParameters() {
  // Camera calibration parameters
  nh.param<string>("cam0/distortion_model",
      cam0_distortion_model, string("radtan"));
  nh.param<string>("cam1/distortion_model",
      cam1_distortion_model, string("radtan"));

  vector<int> cam0_resolution_temp(2);
  nh.getParam("cam0/resolution", cam0_resolution_temp);
  cam0_resolution[0] = cam0_resolution_temp[0];
  cam0_resolution[1] = cam0_resolution_temp[1];

  vector<int> cam1_resolution_temp(2);
  nh.getParam("cam1/resolution", cam1_resolution_temp);
  cam1_resolution[0] = cam1_resolution_temp[0];
  cam1_resolution[1] = cam1_resolution_temp[1];

  vector<double> cam0_intrinsics_temp(4);
  nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
  cam0_intrinsics[0] = cam0_intrinsics_temp[0];
  cam0_intrinsics[1] = cam0_intrinsics_temp[1];
  cam0_intrinsics[2] = cam0_intrinsics_temp[2];
  cam0_intrinsics[3] = cam0_intrinsics_temp[3];

  vector<double> cam1_intrinsics_temp(4);
  nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
  cam1_intrinsics[0] = cam1_intrinsics_temp[0];
  cam1_intrinsics[1] = cam1_intrinsics_temp[1];
  cam1_intrinsics[2] = cam1_intrinsics_temp[2];
  cam1_intrinsics[3] = cam1_intrinsics_temp[3];

  vector<double> cam0_distortion_coeffs_temp(4);
  nh.getParam("cam0/distortion_coeffs",
      cam0_distortion_coeffs_temp);
  cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
  cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
  cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
  cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];

  vector<double> cam1_distortion_coeffs_temp(4);
  nh.getParam("cam1/distortion_coeffs",
      cam1_distortion_coeffs_temp);
  cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
  cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
  cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
  cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];

  cv::Mat     T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
  cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));        // QXC：Rect的最后两个参数分别是width（宽度，列数）和height（高度，行数）
  cv::Vec3d   t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
  R_cam0_imu = R_imu_cam0.t();
  t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;

  cv::Mat T_cam0_cam1 = utils::getTransformCV(nh, "cam1/T_cn_cnm1");
  cv::Mat T_imu_cam1 = T_cam0_cam1 * T_imu_cam0;
  cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
  cv::Vec3d   t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
  R_cam1_imu = R_imu_cam1.t();
  t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;

  // Processor parameters
  nh.param<int>("grid_row", processor_config.grid_row, 4);
  nh.param<int>("grid_col", processor_config.grid_col, 4);
  nh.param<int>("grid_min_feature_num",
      processor_config.grid_min_feature_num, 2);
  nh.param<int>("grid_max_feature_num",
      processor_config.grid_max_feature_num, 4);
  nh.param<int>("pyramid_levels",
      processor_config.pyramid_levels, 3);
  nh.param<int>("patch_size",
      processor_config.patch_size, 31);
  nh.param<int>("fast_threshold",
      processor_config.fast_threshold, 20);
  nh.param<int>("max_iteration",
      processor_config.max_iteration, 30);
  nh.param<double>("track_precision",
      processor_config.track_precision, 0.01);
  nh.param<double>("ransac_threshold",
      processor_config.ransac_threshold, 3);
  nh.param<double>("stereo_threshold",
      processor_config.stereo_threshold, 3);

  ROS_INFO("===========================================");
  ROS_INFO("cam0_resolution: %d, %d",
      cam0_resolution[0], cam0_resolution[1]);
  ROS_INFO("cam0_intrinscs: %f, %f, %f, %f",
      cam0_intrinsics[0], cam0_intrinsics[1],
      cam0_intrinsics[2], cam0_intrinsics[3]);
  ROS_INFO("cam0_distortion_model: %s",
      cam0_distortion_model.c_str());
  ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f",
      cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
      cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);

  ROS_INFO("cam1_resolution: %d, %d",
      cam1_resolution[0], cam1_resolution[1]);
  ROS_INFO("cam1_intrinscs: %f, %f, %f, %f",
      cam1_intrinsics[0], cam1_intrinsics[1],
      cam1_intrinsics[2], cam1_intrinsics[3]);
  ROS_INFO("cam1_distortion_model: %s",
      cam1_distortion_model.c_str());
  ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f",
      cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
      cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);

  cout << R_imu_cam0 << endl;
  cout << t_imu_cam0.t() << endl;

  ROS_INFO("grid_row: %d",
      processor_config.grid_row);
  ROS_INFO("grid_col: %d",
      processor_config.grid_col);
  ROS_INFO("grid_min_feature_num: %d",
      processor_config.grid_min_feature_num);
  ROS_INFO("grid_max_feature_num: %d",
      processor_config.grid_max_feature_num);
  ROS_INFO("pyramid_levels: %d",
      processor_config.pyramid_levels);
  ROS_INFO("patch_size: %d",
      processor_config.patch_size);
  ROS_INFO("fast_threshold: %d",
      processor_config.fast_threshold);
  ROS_INFO("max_iteration: %d",
      processor_config.max_iteration);
  ROS_INFO("track_precision: %f",
      processor_config.track_precision);
  ROS_INFO("ransac_threshold: %f",
      processor_config.ransac_threshold);
  ROS_INFO("stereo_threshold: %f",
      processor_config.stereo_threshold);
  ROS_INFO("===========================================");
  return true;
}

// 声明发布和订阅消息，注册相应的回调函数
bool ImageProcessor::createRosIO() {
  feature_pub = nh.advertise<CameraMeasurement>(
      "features", 3);       // QXC：开始发布名为feature的消息，缓存长度为3
  tracking_info_pub = nh.advertise<TrackingInfo>(
      "tracking_info", 1);  // QXC：开始发布名为tracking_info的消息，缓存长度为1
  image_transport::ImageTransport it(nh);
  debug_stereo_pub = it.advertise("debug_stereo_image", 1);     // QXC：开始发布名为debug_stereo_image的消息，缓存长度为1

  cam0_img_sub.subscribe(nh, "cam0_image", 10);     // QXC：订阅cam0_image消息
  cam1_img_sub.subscribe(nh, "cam1_image", 10);     // QXC：订阅cam1_image消息
  stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);      // QXC：将双目的两帧图像消息结合起来，connectInput的具体解析没找到
  stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);       // QXC：注册stereo_sub的回调函数为本对象的stereoCallback函数
  imu_sub = nh.subscribe("imu", 50,
      &ImageProcessor::imuCallback, this);      // QXC：订阅imu消息，并注册其回调函数为本对象的imuCallback函数

  return true;
}

// 供image_processor_nodelet的onInit调用的初始化函数，将导入参数以及创建各回调函数线程
bool ImageProcessor::initialize() {
  if (!loadParameters()) return false;
  ROS_INFO("Finish loading ROS parameters...");

  // Create feature detector.
  detector_ptr = FastFeatureDetector::create(
      processor_config.fast_threshold);

  if (!createRosIO()) return false;
  ROS_INFO("Finish creating ROS IO...");

  return true;
}

void ImageProcessor::stereoCallback(
    const sensor_msgs::ImageConstPtr& cam0_img,
    const sensor_msgs::ImageConstPtr& cam1_img) {

  //cout << "==================================" << endl;

  // Get the current image.     // QXC：将ros的image消息转换为opencv中的cv::Mat（在cami_curr_img_ptr的成员image中存储），其中cv::Mat为mono8格式，以满足后面的createImagePyramids调用的cv::buildOpticalFlowPyramid函数对参数的要求
  cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
      sensor_msgs::image_encodings::MONO8);
  cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
      sensor_msgs::image_encodings::MONO8);

  // Build the image pyramids once since they're used at multiple places
  createImagePyramids();        // QXC：为两帧图像划分金字塔

  // Detect features in the first frame.
  if (is_first_img) {
    //ros::Time start_time = ros::Time::now();
    initializeFirstFrame();     // QXC：初始化第一批特征：利用了LKT光流法来寻找两帧间的匹配点；利用了对极约束筛选野点；将特征点划分到不同的图像grid中
    //ROS_INFO("Detection time: %f",
    //    (ros::Time::now()-start_time).toSec());
    is_first_img = false;

    // Draw results.
    //start_time = ros::Time::now();
    drawFeaturesStereo();       // QXC：当有其他节点订阅了debug_stereo_image消息时，将双目图像拼接起来并画出特征点位置，作为消息发送出去
    //ROS_INFO("Draw features: %f",
    //    (ros::Time::now()-start_time).toSec());
  } else {
    // Track the feature in the previous image.
    //ros::Time start_time = ros::Time::now();
    trackFeatures();
    //ROS_INFO("Tracking time: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Add new features into the current image.
    //start_time = ros::Time::now();
    addNewFeatures();
    //ROS_INFO("Addition time: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Add new features into the current image.
    //start_time = ros::Time::now();
    pruneGridFeatures();
    //ROS_INFO("Prune grid features: %f",
    //    (ros::Time::now()-start_time).toSec());

    // Draw results.
    //start_time = ros::Time::now();
    drawFeaturesStereo();       // QXC：当有其他节点订阅了debug_stereo_image消息时，将双目图像拼接起来并画出特征点位置，作为消息发送出去
    //ROS_INFO("Draw features: %f",
    //    (ros::Time::now()-start_time).toSec());
  }

  //ros::Time start_time = ros::Time::now();
  //updateFeatureLifetime();
  //ROS_INFO("Statistics: %f",
  //    (ros::Time::now()-start_time).toSec());

  // Publish features in the current image.
  //ros::Time start_time = ros::Time::now();
  publish();        // QXC：发布features和tracking_info消息
  //ROS_INFO("Publishing: %f",
  //    (ros::Time::now()-start_time).toSec());

  // Update the previous image and previous features.
  cam0_prev_img_ptr = cam0_curr_img_ptr;
  prev_features_ptr = curr_features_ptr;
  std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);        // QXC：交换两者内容

  // Initialize the current features to empty vectors.
  curr_features_ptr.reset(new GridFeatures());
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code) {
    (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
  }

  return;
}

// 收到imu消息时，当第一个双目帧还未到来时不处理，其他时候只是压入容器中
void ImageProcessor::imuCallback(
    const sensor_msgs::ImuConstPtr& msg) {
  // Wait for the first image to be set.
  if (is_first_img) return;
  imu_msg_buffer.push_back(*msg);
  return;
}

// 事先为输入图像分割金字塔层，以便后续使用
void ImageProcessor::createImagePyramids() {
  const Mat& curr_cam0_img = cam0_curr_img_ptr->image;
  buildOpticalFlowPyramid(
      curr_cam0_img, curr_cam0_pyramid_,
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels, true, BORDER_REFLECT_101,
      BORDER_CONSTANT, false);      // QXC：该函数来自opencv

  const Mat& curr_cam1_img = cam1_curr_img_ptr->image;
  buildOpticalFlowPyramid(
      curr_cam1_img, curr_cam1_pyramid_,
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels, true, BORDER_REFLECT_101,
      BORDER_CONSTANT, false);
}

// 从cam0的图像中提取FAST特征，并利用LKT光流法在cam1的图像中寻找匹配的像素点，并利用双目外参构成的对极几何约束进行野点筛选。
// 然后根据cam0中所有匹配特征点的位置将它们分配到不同的grid中，按提取FAST特征时的response对每个grid中的特征进行排序，最后将它们存储到相应的类成员变量中（每个grid特征数有限制）。
void ImageProcessor::initializeFirstFrame() {
  // Size of each grid.
  const Mat& img = cam0_curr_img_ptr->image;
  static int grid_height = img.rows / processor_config.grid_row;        // QXC：grid_row和grid_col都为4，意思是将图像划分为4*4=16个grid，
  static int grid_width = img.cols / processor_config.grid_col;         //      这里计算得到每个grid的像素高和宽

  // Detect new features on the frist image.
  vector<KeyPoint> new_features(0);
  detector_ptr->detect(img, new_features);      // QXC：对cam0的图像提取FAST特征，作为LKT光流中“easy to track”的图像点

  // Find the stereo matched points for the newly
  // detected features.
  vector<cv::Point2f> cam0_points(new_features.size());
  for (int i = 0; i < new_features.size(); ++i)
    cam0_points[i] = new_features[i].pt;

  vector<cv::Point2f> cam1_points(0);
  vector<unsigned char> inlier_markers(0);
  stereoMatch(cam0_points, cam1_points, inlier_markers);    // QXC：利用LKT光流法在cam1中找到和cam0中特征匹配的点，然后再利用双目相机对极约束筛选内点

  vector<cv::Point2f> cam0_inliers(0);
  vector<cv::Point2f> cam1_inliers(0);
  vector<float> response_inliers(0);
  for (int i = 0; i < inlier_markers.size(); ++i) {         // QXC：将内点及其response推入vector容器中
    if (inlier_markers[i] == 0) continue;
    cam0_inliers.push_back(cam0_points[i]);
    cam1_inliers.push_back(cam1_points[i]);
    response_inliers.push_back(new_features[i].response);
  }

  // Group the features into grids
  GridFeatures grid_new_features;
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code)      // QXC：为map容器grid_new_features预赋值，其ID表示图像划分的第几个grid（行优先），
      grid_new_features[code] = vector<FeatureMetaData>(0);             //      内容为该格子里的双目特征组成的容器（由cam0中的位置决定在哪个格子）。

  for (int i = 0; i < cam0_inliers.size(); ++i) {       // QXC：根据像素点在cam0中的位置，计算其所在的grid，据此将相应内容存储到grid_new_features的对应位置
    const cv::Point2f& cam0_point = cam0_inliers[i];
    const cv::Point2f& cam1_point = cam1_inliers[i];
    const float& response = response_inliers[i];

    int row = static_cast<int>(cam0_point.y / grid_height);
    int col = static_cast<int>(cam0_point.x / grid_width);
    int code = row*processor_config.grid_col + col;

    FeatureMetaData new_feature;
    new_feature.response = response;
    new_feature.cam0_point = cam0_point;
    new_feature.cam1_point = cam1_point;
    grid_new_features[code].push_back(new_feature);
  }

  // Sort the new features in each grid based on its response.      // QXC：将每个grid中的所有像素点按response由大到小排序
  for (auto& item : grid_new_features)
    std::sort(item.second.begin(), item.second.end(),
        &ImageProcessor::featureCompareByResponse);

  // Collect new features within each grid with high response.
  for (int code = 0; code <         // QXC：向curr_features_ptr指向的map容器中放入初始的特征grid，每个grid中的特征数不能大于grid_min_feature_num
      processor_config.grid_row*processor_config.grid_col; ++code) {
    vector<FeatureMetaData>& features_this_grid = (*curr_features_ptr)[code];       // QXC：获得map容器中存放当前特征grid的vector容器的引用
    vector<FeatureMetaData>& new_features_this_grid = grid_new_features[code];

    for (int k = 0; k < processor_config.grid_min_feature_num &&
        k < new_features_this_grid.size(); ++k) {       // QXC：首次存放特征时，每个grid中的特征数不能大于grid_min_feature_num
      features_this_grid.push_back(new_features_this_grid[k]);
      features_this_grid.back().id = next_feature_id++;
      features_this_grid.back().lifetime = 1;
    }
  }

  return;
}

// 利用输入的前一帧特征点图像坐标、前一帧到当前帧的旋转矩阵以及相机内参，预测当前帧中的特征点图像坐标。
// 作用是给LKT光流一个initial guess。
void ImageProcessor::predictFeatureTracking(
    const vector<cv::Point2f>& input_pts,
    const cv::Matx33f& R_p_c,
    const cv::Vec4d& intrinsics,
    vector<cv::Point2f>& compensated_pts) {

  // Return directly if there are no input features.
  if (input_pts.size() == 0) {
    compensated_pts.clear();
    return;
  }
  compensated_pts.resize(input_pts.size());

  // Intrinsic matrix.
  cv::Matx33f K(
      intrinsics[0], 0.0, intrinsics[2],
      0.0, intrinsics[1], intrinsics[3],
      0.0, 0.0, 1.0);
  cv::Matx33f H = K * R_p_c * K.inv();    // QXC：采用将特征点在前一帧相机系下归一化坐标旋转到当前帧相机系下的方式进行预测

  for (int i = 0; i < input_pts.size(); ++i) {
    cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f);
    cv::Vec3f p2 = H * p1;
    compensated_pts[i].x = p2[0] / p2[2];
    compensated_pts[i].y = p2[1] / p2[2];
  }

  return;
}

void ImageProcessor::trackFeatures() {
  // Size of each grid.
  static int grid_height =
    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
  static int grid_width =
    cam0_curr_img_ptr->image.cols / processor_config.grid_col;

  // Compute a rough relative rotation which takes a vector
  // from the previous frame to the current frame.
  Matx33f cam0_R_p_c;
  Matx33f cam1_R_p_c;
  integrateImuData(cam0_R_p_c, cam1_R_p_c);     // QXC：利用前一帧和当前帧之间的imu数据近似的求解前一帧到当前帧的旋转矩阵

  // Organize the features in the previous image.
  vector<FeatureIDType> prev_ids(0);
  vector<int> prev_lifetime(0);
  vector<Point2f> prev_cam0_points(0);
  vector<Point2f> prev_cam1_points(0);

  for (const auto& item : *prev_features_ptr) {
    for (const auto& prev_feature : item.second) {
      prev_ids.push_back(prev_feature.id);
      prev_lifetime.push_back(prev_feature.lifetime);
      prev_cam0_points.push_back(prev_feature.cam0_point);
      prev_cam1_points.push_back(prev_feature.cam1_point);
    }
  }

  // Number of the features before tracking.
  before_tracking = prev_cam0_points.size();

  // Abort tracking if there is no features in
  // the previous frame.
  if (prev_ids.size() == 0) return;

  // Track features using LK optical flow method.
  vector<Point2f> curr_cam0_points(0);
  vector<unsigned char> track_inliers(0);

  predictFeatureTracking(prev_cam0_points,                // QXC：利用前一帧特征点图像坐标、前一帧到当前帧旋转矩阵以及相机内参，预测当前帧特征图像坐标。
      cam0_R_p_c, cam0_intrinsics, curr_cam0_points);     //      结果用于给LKT光流跟踪提供initial guess。

  calcOpticalFlowPyrLK(
      prev_cam0_pyramid_, curr_cam0_pyramid_,
      prev_cam0_points, curr_cam0_points,
      track_inliers, noArray(),
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels,
      TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
        processor_config.max_iteration,
        processor_config.track_precision),
      cv::OPTFLOW_USE_INITIAL_FLOW);          // QXC：用LKT光流法在当前帧中跟踪前一帧的特征点

  // Mark those tracked points out of the image region
  // as untracked.
  for (int i = 0; i < curr_cam0_points.size(); ++i) {   // QXC：将跟踪到的位于图像外的特征设置为外点
    if (track_inliers[i] == 0) continue;
    if (curr_cam0_points[i].y < 0 ||
        curr_cam0_points[i].y > cam0_curr_img_ptr->image.rows-1 ||
        curr_cam0_points[i].x < 0 ||
        curr_cam0_points[i].x > cam0_curr_img_ptr->image.cols-1)
      track_inliers[i] = 0;
  }

  // Collect the tracked points.
  vector<FeatureIDType> prev_tracked_ids(0);
  vector<int> prev_tracked_lifetime(0);
  vector<Point2f> prev_tracked_cam0_points(0);
  vector<Point2f> prev_tracked_cam1_points(0);
  vector<Point2f> curr_tracked_cam0_points(0);

  removeUnmarkedElements(    // QXC：该函数根据第二个参数（用0标识外点、1标识内点）将第一个参数中的外点剔除，重新构造一个容器传给第三个参数
      prev_ids, track_inliers, prev_tracked_ids);
  removeUnmarkedElements(
      prev_lifetime, track_inliers, prev_tracked_lifetime);
  removeUnmarkedElements(
      prev_cam0_points, track_inliers, prev_tracked_cam0_points);
  removeUnmarkedElements(
      prev_cam1_points, track_inliers, prev_tracked_cam1_points);
  removeUnmarkedElements(
      curr_cam0_points, track_inliers, curr_tracked_cam0_points);

  // Number of features left after tracking.
  after_tracking = curr_tracked_cam0_points.size();

  // Outlier removal involves three steps, which forms a close
  // loop between the previous and current frames of cam0 (left)
  // and cam1 (right). Assuming the stereo matching between the
  // previous cam0 and cam1 images are correct, the three steps are:
  //
  // prev frames cam0 ----------> cam1
  //              |                |
  //              |ransac          |ransac
  //              |   stereo match |
  // curr frames cam0 ----------> cam1
  //
  // 1) Stereo matching between current images of cam0 and cam1.
  // 2) RANSAC between previous and current images of cam0.
  // 3) RANSAC between previous and current images of cam1.
  //
  // For Step 3, tracking between the images is no longer needed.
  // The stereo matching results are directly used in the RANSAC.

  // Step 1: stereo matching.
  vector<Point2f> curr_cam1_points(0);
  vector<unsigned char> match_inliers(0);
  stereoMatch(curr_tracked_cam0_points, curr_cam1_points, match_inliers);   // QXC：利用LKT光流法和双目外参约束，在当前cam1上找到和当前cam0上匹配的特征点

  vector<FeatureIDType> prev_matched_ids(0);
  vector<int> prev_matched_lifetime(0);
  vector<Point2f> prev_matched_cam0_points(0);
  vector<Point2f> prev_matched_cam1_points(0);
  vector<Point2f> curr_matched_cam0_points(0);
  vector<Point2f> curr_matched_cam1_points(0);

  removeUnmarkedElements(
      prev_tracked_ids, match_inliers, prev_matched_ids);                     // QXC：根据基于cam0跟踪结果的当前双目匹配结果，从跟踪结果中剔除外点
  removeUnmarkedElements(
      prev_tracked_lifetime, match_inliers, prev_matched_lifetime);
  removeUnmarkedElements(
      prev_tracked_cam0_points, match_inliers, prev_matched_cam0_points);
  removeUnmarkedElements(
      prev_tracked_cam1_points, match_inliers, prev_matched_cam1_points);
  removeUnmarkedElements(
      curr_tracked_cam0_points, match_inliers, curr_matched_cam0_points);
  removeUnmarkedElements(
      curr_cam1_points, match_inliers, curr_matched_cam1_points);

  // Number of features left after stereo matching.
  after_matching = curr_matched_cam0_points.size();

  // Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1.
  vector<int> cam0_ransac_inliers(0);
  twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points,
      cam0_R_p_c, cam0_intrinsics, cam0_distortion_model,
      cam0_distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam0_ransac_inliers);

  vector<int> cam1_ransac_inliers(0);
  twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
      cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
      cam1_distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam1_ransac_inliers);

  // Number of features after ransac.
  after_ransac = 0;

  for (int i = 0; i < cam0_ransac_inliers.size(); ++i) {
    if (cam0_ransac_inliers[i] == 0 ||
        cam1_ransac_inliers[i] == 0) continue;
    int row = static_cast<int>(
        curr_matched_cam0_points[i].y / grid_height);
    int col = static_cast<int>(
        curr_matched_cam0_points[i].x / grid_width);
    int code = row*processor_config.grid_col + col;
    (*curr_features_ptr)[code].push_back(FeatureMetaData());

    FeatureMetaData& grid_new_feature = (*curr_features_ptr)[code].back();
    grid_new_feature.id = prev_matched_ids[i];
    grid_new_feature.lifetime = ++prev_matched_lifetime[i];
    grid_new_feature.cam0_point = curr_matched_cam0_points[i];
    grid_new_feature.cam1_point = curr_matched_cam1_points[i];

    ++after_ransac;
  }

  // Compute the tracking rate.
  int prev_feature_num = 0;
  for (const auto& item : *prev_features_ptr)
    prev_feature_num += item.second.size();

  int curr_feature_num = 0;
  for (const auto& item : *curr_features_ptr)
    curr_feature_num += item.second.size();

  ROS_INFO_THROTTLE(0.5,
      "\033[0;32m candidates: %d; track: %d; match: %d; ransac: %d/%d=%f\033[0m",
      before_tracking, after_tracking, after_matching,
      curr_feature_num, prev_feature_num,
      static_cast<double>(curr_feature_num)/
      (static_cast<double>(prev_feature_num)+1e-5));
  //printf(
  //    "\033[0;32m candidates: %d; raw track: %d; stereo match: %d; ransac: %d/%d=%f\033[0m\n",
  //    before_tracking, after_tracking, after_matching,
  //    curr_feature_num, prev_feature_num,
  //    static_cast<double>(curr_feature_num)/
  //    (static_cast<double>(prev_feature_num)+1e-5));

  return;
}

// 进行双目相机特征-像素点匹配，先用LKT光流法在cam1图像上找到和cam0图像特征点匹配的像素点（这个过程中会进行一轮筛选），
// 然后再利用双目相机外参计算得到本质矩阵，利用对极约束再一次筛选匹配点
void ImageProcessor::stereoMatch(
    const vector<cv::Point2f>& cam0_points,
    vector<cv::Point2f>& cam1_points,
    vector<unsigned char>& inlier_markers) {

  if (cam0_points.size() == 0) return;

  if(cam1_points.size() == 0) {
    // Initialize cam1_points by projecting cam0_points to cam1 using the
    // rotation from stereo extrinsics
    const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
    vector<cv::Point2f> cam0_points_undistorted;
    undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
                    cam0_distortion_coeffs, cam0_points_undistorted,
                    R_cam0_cam1);       // QXC：将cam0的特征点坐标转换成相机系下的归一化坐标（去畸变），然后将这些坐标矢量投影到cam1的相机系下（这样是为了设定一个初值）
    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
                                cam1_distortion_model, cam1_distortion_coeffs);     // QXC：利用前面得到的cam1相机系下的cam0特征点归一化坐标直接计算得到cam1的特征图像坐标
  }

  // Track features using LK optical flow method.
  calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
      cam0_points, cam1_points,    // QXC：当OPTFLOW_USE_INITIAL_FLOW参数传入时，cam1_points传入的值作为cam1中特征位置初值，但它最终会用来存放cam1特征点输出值
      inlier_markers, noArray(),
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels,
      TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
                   processor_config.max_iteration,       // QXC：max_iteration用来限制每层金字塔上LKT光流跟踪的迭代次数，即畸变光流未收敛，当到达迭代次数时也停止迭代。
                   processor_config.track_precision),    //      track_precision用来限制每层金字塔LKT光流跟踪的迭代终止阈值，即当某次迭代光流小于该阈值时则停止迭代。
      cv::OPTFLOW_USE_INITIAL_FLOW);    // QXC：用LK光流法进行特征点匹配，cam0_points为cam0中的特征点位置，cam1_points为cam1中的特征点（in为初值，out为最终结果）

  // Mark those tracked points out of the image region
  // as untracked.
  for (int i = 0; i < cam1_points.size(); ++i) {        // QXC：去除落在图像像素坐标范围外的点
    if (inlier_markers[i] == 0) continue;
    if (cam1_points[i].y < 0 ||
        cam1_points[i].y > cam1_curr_img_ptr->image.rows-1 ||
        cam1_points[i].x < 0 ||
        cam1_points[i].x > cam1_curr_img_ptr->image.cols-1)
      inlier_markers[i] = 0;
  }

  // Compute the relative rotation between the cam0         // QXC：用外参标定结果计算cam0和cam1之间的本质矩阵
  // frame and cam1 frame.
  const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
  const cv::Vec3d t_cam0_cam1 = R_cam1_imu.t() * (t_cam0_imu-t_cam1_imu);
  // Compute the essential matrix.
  const cv::Matx33d t_cam0_cam1_hat(
      0.0, -t_cam0_cam1[2], t_cam0_cam1[1],
      t_cam0_cam1[2], 0.0, -t_cam0_cam1[0],
      -t_cam0_cam1[1], t_cam0_cam1[0], 0.0);
  const cv::Matx33d E = t_cam0_cam1_hat * R_cam0_cam1;

  // Further remove outliers based on the known
  // essential matrix.
  vector<cv::Point2f> cam0_points_undistorted(0);
  vector<cv::Point2f> cam1_points_undistorted(0);
  undistortPoints(
      cam0_points, cam0_intrinsics, cam0_distortion_model,
      cam0_distortion_coeffs, cam0_points_undistorted);     // QXC：计算cam0特征点的去畸变的相机系归一化坐标
  undistortPoints(
      cam1_points, cam1_intrinsics, cam1_distortion_model,
      cam1_distortion_coeffs, cam1_points_undistorted);     // QXC：计算cam1特征点的去畸变的相机系归一化坐标

  double norm_pixel_unit = 4.0 / (
      cam0_intrinsics[0]+cam0_intrinsics[1]+
      cam1_intrinsics[0]+cam1_intrinsics[1]);       // ？QXC：这里将两个相机的两组焦距这样运算是要干嘛？？

  for (int i = 0; i < cam0_points_undistorted.size(); ++i) {    // QXC：细节没看懂，总之是根据阈值将对极约束不佳的点剔除了
    if (inlier_markers[i] == 0) continue;
    cv::Vec3d pt0(cam0_points_undistorted[i].x,
        cam0_points_undistorted[i].y, 1.0);
    cv::Vec3d pt1(cam1_points_undistorted[i].x,
        cam1_points_undistorted[i].y, 1.0);
    cv::Vec3d epipolar_line = E * pt0;
    double error = fabs((pt1.t() * epipolar_line)[0]) / sqrt(
        epipolar_line[0]*epipolar_line[0]+
        epipolar_line[1]*epipolar_line[1]);         // ？QXC：没看懂这个误差项表达的含义！(pt1.t() * epipolar_line)[0]已经表示对极约束本该为0的项了，分母是什么意思？
    if (error > processor_config.stereo_threshold*norm_pixel_unit)
      inlier_markers[i] = 0;
  }

  return;
}

void ImageProcessor::addNewFeatures() {
  const Mat& curr_img = cam0_curr_img_ptr->image;

  // Size of each grid.
  static int grid_height =
    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
  static int grid_width =
    cam0_curr_img_ptr->image.cols / processor_config.grid_col;

  // Create a mask to avoid redetecting existing features.
  Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));

  for (const auto& features : *curr_features_ptr) {
    for (const auto& feature : features.second) {
      const int y = static_cast<int>(feature.cam0_point.y);
      const int x = static_cast<int>(feature.cam0_point.x);

      int up_lim = y-2, bottom_lim = y+3,
          left_lim = x-2, right_lim = x+3;
      if (up_lim < 0) up_lim = 0;
      if (bottom_lim > curr_img.rows) bottom_lim = curr_img.rows;
      if (left_lim < 0) left_lim = 0;
      if (right_lim > curr_img.cols) right_lim = curr_img.cols;

      Range row_range(up_lim, bottom_lim);
      Range col_range(left_lim, right_lim);
      mask(row_range, col_range) = 0;
    }
  }

  // Detect new features.
  vector<KeyPoint> new_features(0);
  detector_ptr->detect(curr_img, new_features, mask);

  // Collect the new detected features based on the grid.
  // Select the ones with top response within each grid afterwards.
  vector<vector<KeyPoint> > new_feature_sieve(
      processor_config.grid_row*processor_config.grid_col);
  for (const auto& feature : new_features) {
    int row = static_cast<int>(feature.pt.y / grid_height);
    int col = static_cast<int>(feature.pt.x / grid_width);
    new_feature_sieve[
      row*processor_config.grid_col+col].push_back(feature);
  }

  new_features.clear();
  for (auto& item : new_feature_sieve) {
    if (item.size() > processor_config.grid_max_feature_num) {
      std::sort(item.begin(), item.end(),
          &ImageProcessor::keyPointCompareByResponse);
      item.erase(
          item.begin()+processor_config.grid_max_feature_num, item.end());
    }
    new_features.insert(new_features.end(), item.begin(), item.end());
  }

  int detected_new_features = new_features.size();

  // Find the stereo matched points for the newly
  // detected features.
  vector<cv::Point2f> cam0_points(new_features.size());
  for (int i = 0; i < new_features.size(); ++i)
    cam0_points[i] = new_features[i].pt;

  vector<cv::Point2f> cam1_points(0);
  vector<unsigned char> inlier_markers(0);
  stereoMatch(cam0_points, cam1_points, inlier_markers);

  vector<cv::Point2f> cam0_inliers(0);
  vector<cv::Point2f> cam1_inliers(0);
  vector<float> response_inliers(0);
  for (int i = 0; i < inlier_markers.size(); ++i) {
    if (inlier_markers[i] == 0) continue;
    cam0_inliers.push_back(cam0_points[i]);
    cam1_inliers.push_back(cam1_points[i]);
    response_inliers.push_back(new_features[i].response);
  }

  int matched_new_features = cam0_inliers.size();

  if (matched_new_features < 5 &&
      static_cast<double>(matched_new_features)/
      static_cast<double>(detected_new_features) < 0.1)
    ROS_WARN("Images at [%f] seems unsynced...",
        cam0_curr_img_ptr->header.stamp.toSec());

  // Group the features into grids
  GridFeatures grid_new_features;
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code)
      grid_new_features[code] = vector<FeatureMetaData>(0);

  for (int i = 0; i < cam0_inliers.size(); ++i) {
    const cv::Point2f& cam0_point = cam0_inliers[i];
    const cv::Point2f& cam1_point = cam1_inliers[i];
    const float& response = response_inliers[i];

    int row = static_cast<int>(cam0_point.y / grid_height);
    int col = static_cast<int>(cam0_point.x / grid_width);
    int code = row*processor_config.grid_col + col;

    FeatureMetaData new_feature;
    new_feature.response = response;
    new_feature.cam0_point = cam0_point;
    new_feature.cam1_point = cam1_point;
    grid_new_features[code].push_back(new_feature);
  }

  // Sort the new features in each grid based on its response.
  for (auto& item : grid_new_features)
    std::sort(item.second.begin(), item.second.end(),
        &ImageProcessor::featureCompareByResponse);

  int new_added_feature_num = 0;
  // Collect new features within each grid with high response.
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code) {
    vector<FeatureMetaData>& features_this_grid = (*curr_features_ptr)[code];
    vector<FeatureMetaData>& new_features_this_grid = grid_new_features[code];

    if (features_this_grid.size() >=
        processor_config.grid_min_feature_num) continue;

    int vacancy_num = processor_config.grid_min_feature_num -
      features_this_grid.size();
    for (int k = 0;
        k < vacancy_num && k < new_features_this_grid.size(); ++k) {
      features_this_grid.push_back(new_features_this_grid[k]);
      features_this_grid.back().id = next_feature_id++;
      features_this_grid.back().lifetime = 1;

      ++new_added_feature_num;
    }
  }

  //printf("\033[0;33m detected: %d; matched: %d; new added feature: %d\033[0m\n",
  //    detected_new_features, matched_new_features, new_added_feature_num);

  return;
}

void ImageProcessor::pruneGridFeatures() {
  for (auto& item : *curr_features_ptr) {
    auto& grid_features = item.second;
    // Continue if the number of features in this grid does
    // not exceed the upper bound.
    if (grid_features.size() <=
        processor_config.grid_max_feature_num) continue;
    std::sort(grid_features.begin(), grid_features.end(),
        &ImageProcessor::featureCompareByLifetime);
    grid_features.erase(grid_features.begin()+
        processor_config.grid_max_feature_num,
        grid_features.end());
  }
  return;
}

// 根据图像点的像素坐标、相机内参矩阵、畸变模型和系数，得到图像点在相机系下去畸变的归一化坐标，然后根据rectification_matrix对这些矢量进行旋转，再根据new_intrinsics投影到新的图像坐标系下
void ImageProcessor::undistortPoints(
    const vector<cv::Point2f>& pts_in,
    const cv::Vec4d& intrinsics,
    const string& distortion_model,
    const cv::Vec4d& distortion_coeffs,
    vector<cv::Point2f>& pts_out,
    const cv::Matx33d &rectification_matrix,
    const cv::Vec4d &new_intrinsics) {

  if (pts_in.size() == 0) return;

  const cv::Matx33d K(
      intrinsics[0], 0.0, intrinsics[2],
      0.0, intrinsics[1], intrinsics[3],
      0.0, 0.0, 1.0);

  const cv::Matx33d K_new(
      new_intrinsics[0], 0.0, new_intrinsics[2],
      0.0, new_intrinsics[1], new_intrinsics[3],
      0.0, 0.0, 1.0);

  if (distortion_model == "radtan") {
    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                        rectification_matrix, K_new);       // QXC：该函数计算原始图像点的归一化相机坐标，然后消除畸变影响，然后对其做旋转变换，然后投影到新图像坐标系下（如果K_new不为单位阵的话）
  } else if (distortion_model == "equidistant") {
    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                 rectification_matrix, K_new);
  } else {
    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
                  distortion_model.c_str());
    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                        rectification_matrix, K_new);
  }

  return;
}

// 根据图像点的相机系归一化齐次坐标、内参矩阵、相机畸变模型以及畸变参数，计算图像点像素坐标
vector<cv::Point2f> ImageProcessor::distortPoints(
    const vector<cv::Point2f>& pts_in,
    const cv::Vec4d& intrinsics,
    const string& distortion_model,
    const cv::Vec4d& distortion_coeffs) {

  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
                      0.0, intrinsics[1], intrinsics[3],
                      0.0, 0.0, 1.0);

  vector<cv::Point2f> pts_out;
  if (distortion_model == "radtan") {
    vector<cv::Point3f> homogenous_pts;
    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);     // QXC：得到齐次坐标（也就是多加一维1）
    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
                      distortion_coeffs, pts_out);      // QXC：根据相机坐标系下图像点归一化齐次坐标、内参矩阵以及畸变参数，得到图像点的像素坐标
  } else if (distortion_model == "equidistant") {
    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
  } else {
    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                  distortion_model.c_str());
    vector<cv::Point3f> homogenous_pts;
    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
                      distortion_coeffs, pts_out);
  }

  return pts_out;
}

// 利用当前帧和上一帧之间的imu数据，用求平均旋转矢量的方式求前一帧到当前帧的旋转矩阵
void ImageProcessor::integrateImuData(
    Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) {
  // Find the start and the end limit within the imu msg buffer.
  auto begin_iter = imu_msg_buffer.begin();
  while (begin_iter != imu_msg_buffer.end()) {
    if ((begin_iter->header.stamp-
          cam0_prev_img_ptr->header.stamp).toSec() < -0.01)
      ++begin_iter;
    else
      break;
  }

  auto end_iter = begin_iter;
  while (end_iter != imu_msg_buffer.end()) {
    if ((end_iter->header.stamp-
          cam0_curr_img_ptr->header.stamp).toSec() < 0.005)
      ++end_iter;
    else
      break;
  }

  // Compute the mean angular velocity in the IMU frame.
  Vec3f mean_ang_vel(0.0, 0.0, 0.0);
  for (auto iter = begin_iter; iter < end_iter; ++iter)
    mean_ang_vel += Vec3f(iter->angular_velocity.x,
        iter->angular_velocity.y, iter->angular_velocity.z);

  if (end_iter-begin_iter > 0)
    mean_ang_vel *= 1.0f / (end_iter-begin_iter);

  // Transform the mean angular velocity from the IMU
  // frame to the cam0 and cam1 frames.
  Vec3f cam0_mean_ang_vel = R_cam0_imu.t() * mean_ang_vel;
  Vec3f cam1_mean_ang_vel = R_cam1_imu.t() * mean_ang_vel;

  // Compute the relative rotation.
  double dtime = (cam0_curr_img_ptr->header.stamp-
      cam0_prev_img_ptr->header.stamp).toSec();
  Rodrigues(cam0_mean_ang_vel*dtime, cam0_R_p_c);   // QXC：罗德里格斯公式，给出prev系下的旋转矢量，输出旋转矩阵R_curr2Prev
  Rodrigues(cam1_mean_ang_vel*dtime, cam1_R_p_c);
  cam0_R_p_c = cam0_R_p_c.t();                      // QXC：要得到R_Prev2curr还要进行一次转置
  cam1_R_p_c = cam1_R_p_c.t();

  // Delete the useless and used imu messages.
  imu_msg_buffer.erase(imu_msg_buffer.begin(), end_iter);
  return;
}

void ImageProcessor::rescalePoints(
    vector<Point2f>& pts1, vector<Point2f>& pts2,
    float& scaling_factor) {

  scaling_factor = 0.0f;

  for (int i = 0; i < pts1.size(); ++i) {
    scaling_factor += sqrt(pts1[i].dot(pts1[i]));
    scaling_factor += sqrt(pts2[i].dot(pts2[i]));
  }

  scaling_factor = (pts1.size()+pts2.size()) /
    scaling_factor * sqrt(2.0f);

  for (int i = 0; i < pts1.size(); ++i) {
    pts1[i] *= scaling_factor;
    pts2[i] *= scaling_factor;
  }

  return;
}

void ImageProcessor::twoPointRansac(
    const vector<Point2f>& pts1, const vector<Point2f>& pts2,
    const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics,
    const std::string& distortion_model,
    const cv::Vec4d& distortion_coeffs,
    const double& inlier_error,
    const double& success_probability,
    vector<int>& inlier_markers) {

  // Check the size of input point size.
  if (pts1.size() != pts2.size())
    ROS_ERROR("Sets of different size (%lu and %lu) are used...",
        pts1.size(), pts2.size());

  double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);
  int iter_num = static_cast<int>(
      ceil(log(1-success_probability) / log(1-0.7*0.7)));

  // Initially, mark all points as inliers.
  inlier_markers.clear();
  inlier_markers.resize(pts1.size(), 1);

  // Undistort all the points.
  vector<Point2f> pts1_undistorted(pts1.size());
  vector<Point2f> pts2_undistorted(pts2.size());
  undistortPoints(
      pts1, intrinsics, distortion_model,
      distortion_coeffs, pts1_undistorted);
  undistortPoints(
      pts2, intrinsics, distortion_model,
      distortion_coeffs, pts2_undistorted);

  // Compenstate the points in the previous image with
  // the relative rotation.
  for (auto& pt : pts1_undistorted) {
    Vec3f pt_h(pt.x, pt.y, 1.0f);
    //Vec3f pt_hc = dR * pt_h;
    Vec3f pt_hc = R_p_c * pt_h;
    pt.x = pt_hc[0];
    pt.y = pt_hc[1];
  }

  // Normalize the points to gain numerical stability.
  float scaling_factor = 0.0f;
  rescalePoints(pts1_undistorted, pts2_undistorted, scaling_factor);
  norm_pixel_unit *= scaling_factor;

  // Compute the difference between previous and current points,
  // which will be used frequently later.
  vector<Point2d> pts_diff(pts1_undistorted.size());
  for (int i = 0; i < pts1_undistorted.size(); ++i)
    pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];

  // Mark the point pairs with large difference directly.
  // BTW, the mean distance of the rest of the point pairs
  // are computed.
  double mean_pt_distance = 0.0;
  int raw_inlier_cntr = 0;
  for (int i = 0; i < pts_diff.size(); ++i) {
    double distance = sqrt(pts_diff[i].dot(pts_diff[i]));
    // 25 pixel distance is a pretty large tolerance for normal motion.
    // However, to be used with aggressive motion, this tolerance should
    // be increased significantly to match the usage.
    if (distance > 50.0*norm_pixel_unit) {
      inlier_markers[i] = 0;
    } else {
      mean_pt_distance += distance;
      ++raw_inlier_cntr;
    }
  }
  mean_pt_distance /= raw_inlier_cntr;

  // If the current number of inliers is less than 3, just mark
  // all input as outliers. This case can happen with fast
  // rotation where very few features are tracked.
  if (raw_inlier_cntr < 3) {
    for (auto& marker : inlier_markers) marker = 0;
    return;
  }

  // Before doing 2-point RANSAC, we have to check if the motion
  // is degenerated, meaning that there is no translation between
  // the frames, in which case, the model of the RANSAC does not
  // work. If so, the distance between the matched points will
  // be almost 0.
  //if (mean_pt_distance < inlier_error*norm_pixel_unit) {
  if (mean_pt_distance < norm_pixel_unit) {
    //ROS_WARN_THROTTLE(1.0, "Degenerated motion...");
    for (int i = 0; i < pts_diff.size(); ++i) {
      if (inlier_markers[i] == 0) continue;
      if (sqrt(pts_diff[i].dot(pts_diff[i])) >
          inlier_error*norm_pixel_unit)
        inlier_markers[i] = 0;
    }
    return;
  }

  // In the case of general motion, the RANSAC model can be applied.
  // The three column corresponds to tx, ty, and tz respectively.
  MatrixXd coeff_t(pts_diff.size(), 3);
  for (int i = 0; i < pts_diff.size(); ++i) {
    coeff_t(i, 0) = pts_diff[i].y;
    coeff_t(i, 1) = -pts_diff[i].x;
    coeff_t(i, 2) = pts1_undistorted[i].x*pts2_undistorted[i].y -
      pts1_undistorted[i].y*pts2_undistorted[i].x;
  }

  vector<int> raw_inlier_idx;
  for (int i = 0; i < inlier_markers.size(); ++i) {
    if (inlier_markers[i] != 0)
      raw_inlier_idx.push_back(i);
  }

  vector<int> best_inlier_set;
  double best_error = 1e10;
  random_numbers::RandomNumberGenerator random_gen;

  for (int iter_idx = 0; iter_idx < iter_num; ++iter_idx) {
    // Randomly select two point pairs.
    // Although this is a weird way of selecting two pairs, but it
    // is able to efficiently avoid selecting repetitive pairs.
    int pair_idx1 = raw_inlier_idx[random_gen.uniformInteger(
        0, raw_inlier_idx.size()-1)];
    int idx_diff = random_gen.uniformInteger(
        1, raw_inlier_idx.size()-1);
    int pair_idx2 = pair_idx1+idx_diff < raw_inlier_idx.size() ?
      pair_idx1+idx_diff : pair_idx1+idx_diff-raw_inlier_idx.size();

    // Construct the model;
    Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
    Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
    Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
    vector<double> coeff_l1_norm(3);
    coeff_l1_norm[0] = coeff_tx.lpNorm<1>();
    coeff_l1_norm[1] = coeff_ty.lpNorm<1>();
    coeff_l1_norm[2] = coeff_tz.lpNorm<1>();
    int base_indicator = min_element(coeff_l1_norm.begin(),
        coeff_l1_norm.end())-coeff_l1_norm.begin();

    Vector3d model(0.0, 0.0, 0.0);
    if (base_indicator == 0) {
      Matrix2d A;
      A << coeff_ty, coeff_tz;
      Vector2d solution = A.inverse() * (-coeff_tx);
      model(0) = 1.0;
      model(1) = solution(0);
      model(2) = solution(1);
    } else if (base_indicator ==1) {
      Matrix2d A;
      A << coeff_tx, coeff_tz;
      Vector2d solution = A.inverse() * (-coeff_ty);
      model(0) = solution(0);
      model(1) = 1.0;
      model(2) = solution(1);
    } else {
      Matrix2d A;
      A << coeff_tx, coeff_ty;
      Vector2d solution = A.inverse() * (-coeff_tz);
      model(0) = solution(0);
      model(1) = solution(1);
      model(2) = 1.0;
    }

    // Find all the inliers among point pairs.
    VectorXd error = coeff_t * model;

    vector<int> inlier_set;
    for (int i = 0; i < error.rows(); ++i) {
      if (inlier_markers[i] == 0) continue;
      if (std::abs(error(i)) < inlier_error*norm_pixel_unit)
        inlier_set.push_back(i);
    }

    // If the number of inliers is small, the current
    // model is probably wrong.
    if (inlier_set.size() < 0.2*pts1_undistorted.size())
      continue;

    // Refit the model using all of the possible inliers.
    VectorXd coeff_tx_better(inlier_set.size());
    VectorXd coeff_ty_better(inlier_set.size());
    VectorXd coeff_tz_better(inlier_set.size());
    for (int i = 0; i < inlier_set.size(); ++i) {
      coeff_tx_better(i) = coeff_t(inlier_set[i], 0);
      coeff_ty_better(i) = coeff_t(inlier_set[i], 1);
      coeff_tz_better(i) = coeff_t(inlier_set[i], 2);
    }

    Vector3d model_better(0.0, 0.0, 0.0);
    if (base_indicator == 0) {
      MatrixXd A(inlier_set.size(), 2);
      A << coeff_ty_better, coeff_tz_better;
      Vector2d solution =
          (A.transpose() * A).inverse() * A.transpose() * (-coeff_tx_better);
      model_better(0) = 1.0;
      model_better(1) = solution(0);
      model_better(2) = solution(1);
    } else if (base_indicator ==1) {
      MatrixXd A(inlier_set.size(), 2);
      A << coeff_tx_better, coeff_tz_better;
      Vector2d solution =
          (A.transpose() * A).inverse() * A.transpose() * (-coeff_ty_better);
      model_better(0) = solution(0);
      model_better(1) = 1.0;
      model_better(2) = solution(1);
    } else {
      MatrixXd A(inlier_set.size(), 2);
      A << coeff_tx_better, coeff_ty_better;
      Vector2d solution =
          (A.transpose() * A).inverse() * A.transpose() * (-coeff_tz_better);
      model_better(0) = solution(0);
      model_better(1) = solution(1);
      model_better(2) = 1.0;
    }

    // Compute the error and upate the best model if possible.
    VectorXd new_error = coeff_t * model_better;

    double this_error = 0.0;
    for (const auto& inlier_idx : inlier_set)
      this_error += std::abs(new_error(inlier_idx));
    this_error /= inlier_set.size();

    if (inlier_set.size() > best_inlier_set.size()) {
      best_error = this_error;
      best_inlier_set = inlier_set;
    }
  }

  // Fill in the markers.
  inlier_markers.clear();
  inlier_markers.resize(pts1.size(), 0);
  for (const auto& inlier_idx : best_inlier_set)
    inlier_markers[inlier_idx] = 1;

  //printf("inlier ratio: %lu/%lu\n",
  //    best_inlier_set.size(), inlier_markers.size());

  return;
}

// 发布features和tracking_info消息
void ImageProcessor::publish() {

  // Publish features.
  CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
  feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;

  vector<FeatureIDType> curr_ids(0);
  vector<Point2f> curr_cam0_points(0);
  vector<Point2f> curr_cam1_points(0);

  for (const auto& grid_features : (*curr_features_ptr)) {
    for (const auto& feature : grid_features.second) {
      curr_ids.push_back(feature.id);
      curr_cam0_points.push_back(feature.cam0_point);
      curr_cam1_points.push_back(feature.cam1_point);
    }
  }

  vector<Point2f> curr_cam0_points_undistorted(0);
  vector<Point2f> curr_cam1_points_undistorted(0);

  undistortPoints(
      curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
      cam0_distortion_coeffs, curr_cam0_points_undistorted);
  undistortPoints(
      curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
      cam1_distortion_coeffs, curr_cam1_points_undistorted);

  for (int i = 0; i < curr_ids.size(); ++i) {
    feature_msg_ptr->features.push_back(FeatureMeasurement());
    feature_msg_ptr->features[i].id = curr_ids[i];
    feature_msg_ptr->features[i].u0 = curr_cam0_points_undistorted[i].x;
    feature_msg_ptr->features[i].v0 = curr_cam0_points_undistorted[i].y;
    feature_msg_ptr->features[i].u1 = curr_cam1_points_undistorted[i].x;
    feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
  }

  feature_pub.publish(feature_msg_ptr);     // QXC：发布双目两帧图像畸变校正过的特征归一化相机系坐标（只含XY轴）

  // Publish tracking info.
  TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
  tracking_info_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
  tracking_info_msg_ptr->before_tracking = before_tracking;
  tracking_info_msg_ptr->after_tracking = after_tracking;
  tracking_info_msg_ptr->after_matching = after_matching;
  tracking_info_msg_ptr->after_ransac = after_ransac;
  tracking_info_pub.publish(tracking_info_msg_ptr);     // QXC：发布消息，记录了上两帧图像（双目所以两帧）的（未跟踪的）、跟踪后的、匹配后的以及RANSAC后的特征点数目

  return;
}

void ImageProcessor::drawFeaturesMono() {
  // Colors for different features.
  Scalar tracked(0, 255, 0);
  Scalar new_feature(0, 255, 255);

  static int grid_height =
    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
  static int grid_width =
    cam0_curr_img_ptr->image.cols / processor_config.grid_col;

  // Create an output image.
  int img_height = cam0_curr_img_ptr->image.rows;
  int img_width = cam0_curr_img_ptr->image.cols;
  Mat out_img(img_height, img_width, CV_8UC3);
  cvtColor(cam0_curr_img_ptr->image, out_img, CV_GRAY2RGB);

  // Draw grids on the image.
  for (int i = 1; i < processor_config.grid_row; ++i) {
    Point pt1(0, i*grid_height);
    Point pt2(img_width, i*grid_height);
    line(out_img, pt1, pt2, Scalar(255, 0, 0));
  }
  for (int i = 1; i < processor_config.grid_col; ++i) {
    Point pt1(i*grid_width, 0);
    Point pt2(i*grid_width, img_height);
    line(out_img, pt1, pt2, Scalar(255, 0, 0));
  }

  // Collect features ids in the previous frame.
  vector<FeatureIDType> prev_ids(0);
  for (const auto& grid_features : *prev_features_ptr)
    for (const auto& feature : grid_features.second)
      prev_ids.push_back(feature.id);

  // Collect feature points in the previous frame.
  map<FeatureIDType, Point2f> prev_points;
  for (const auto& grid_features : *prev_features_ptr)
    for (const auto& feature : grid_features.second)
      prev_points[feature.id] = feature.cam0_point;

  // Collect feature points in the current frame.
  map<FeatureIDType, Point2f> curr_points;
  for (const auto& grid_features : *curr_features_ptr)
    for (const auto& feature : grid_features.second)
      curr_points[feature.id] = feature.cam0_point;

  // Draw tracked features.
  for (const auto& id : prev_ids) {
    if (prev_points.find(id) != prev_points.end() &&
        curr_points.find(id) != curr_points.end()) {
      cv::Point2f prev_pt = prev_points[id];
      cv::Point2f curr_pt = curr_points[id];
      circle(out_img, curr_pt, 3, tracked);
      line(out_img, prev_pt, curr_pt, tracked, 1);

      prev_points.erase(id);
      curr_points.erase(id);
    }
  }

  // Draw new features.
  for (const auto& new_curr_point : curr_points) {
    cv::Point2f pt = new_curr_point.second;
    circle(out_img, pt, 3, new_feature, -1);
  }

  imshow("Feature", out_img);
  waitKey(5);
}

// 当有（其他）节点（应该就是rviz）订阅了debug_stereo_imagex消息时，将当前双目cam拍到的两帧图按左右顺序拼接为一张image，并在该image上分别用不同颜色绘制跟踪的特征点和新特征点。
// 这个画了特征点的image最后会作为debug_stereo_image消息的内容发送出去。
void ImageProcessor::drawFeaturesStereo() {

  if(debug_stereo_pub.getNumSubscribers() > 0)      // QXC：当有其他节点订阅debug_stereo_image消息时才会绘制特征（应该是rviz节点才会订阅该消息）
  {
    // Colors for different features.
    Scalar tracked(0, 255, 0);
    Scalar new_feature(0, 255, 255);

    static int grid_height =
      cam0_curr_img_ptr->image.rows / processor_config.grid_row;
    static int grid_width =
      cam0_curr_img_ptr->image.cols / processor_config.grid_col;

    // Create an output image.
    int img_height = cam0_curr_img_ptr->image.rows;
    int img_width = cam0_curr_img_ptr->image.cols;
    Mat out_img(img_height, img_width*2, CV_8UC3);
    cvtColor(cam0_curr_img_ptr->image,
             out_img.colRange(0, img_width), CV_GRAY2RGB);
    cvtColor(cam1_curr_img_ptr->image,
             out_img.colRange(img_width, img_width*2), CV_GRAY2RGB);

    // Draw grids on the image.
    for (int i = 1; i < processor_config.grid_row; ++i) {
      Point pt1(0, i*grid_height);
      Point pt2(img_width*2, i*grid_height);
      line(out_img, pt1, pt2, Scalar(255, 0, 0));
    }
    for (int i = 1; i < processor_config.grid_col; ++i) {
      Point pt1(i*grid_width, 0);
      Point pt2(i*grid_width, img_height);
      line(out_img, pt1, pt2, Scalar(255, 0, 0));
    }
    for (int i = 1; i < processor_config.grid_col; ++i) {
      Point pt1(i*grid_width+img_width, 0);
      Point pt2(i*grid_width+img_width, img_height);
      line(out_img, pt1, pt2, Scalar(255, 0, 0));
    }

    // Collect features ids in the previous frame.
    vector<FeatureIDType> prev_ids(0);
    for (const auto& grid_features : *prev_features_ptr)
      for (const auto& feature : grid_features.second)
        prev_ids.push_back(feature.id);

    // Collect feature points in the previous frame.
    map<FeatureIDType, Point2f> prev_cam0_points;
    map<FeatureIDType, Point2f> prev_cam1_points;
    for (const auto& grid_features : *prev_features_ptr)
      for (const auto& feature : grid_features.second) {
        prev_cam0_points[feature.id] = feature.cam0_point;
        prev_cam1_points[feature.id] = feature.cam1_point;
      }

    // Collect feature points in the current frame.
    map<FeatureIDType, Point2f> curr_cam0_points;
    map<FeatureIDType, Point2f> curr_cam1_points;
    for (const auto& grid_features : *curr_features_ptr)
      for (const auto& feature : grid_features.second) {
        curr_cam0_points[feature.id] = feature.cam0_point;
        curr_cam1_points[feature.id] = feature.cam1_point;
      }

    // Draw tracked features.
    for (const auto& id : prev_ids) {
      if (prev_cam0_points.find(id) != prev_cam0_points.end() &&
          curr_cam0_points.find(id) != curr_cam0_points.end()) {
        cv::Point2f prev_pt0 = prev_cam0_points[id];
        cv::Point2f prev_pt1 = prev_cam1_points[id] + Point2f(img_width, 0.0);
        cv::Point2f curr_pt0 = curr_cam0_points[id];
        cv::Point2f curr_pt1 = curr_cam1_points[id] + Point2f(img_width, 0.0);

        circle(out_img, curr_pt0, 3, tracked, -1);
        circle(out_img, curr_pt1, 3, tracked, -1);
        line(out_img, prev_pt0, curr_pt0, tracked, 1);
        line(out_img, prev_pt1, curr_pt1, tracked, 1);

        prev_cam0_points.erase(id);
        prev_cam1_points.erase(id);
        curr_cam0_points.erase(id);
        curr_cam1_points.erase(id);
      }
    }

    // Draw new features.
    for (const auto& new_cam0_point : curr_cam0_points) {
      cv::Point2f pt0 = new_cam0_point.second;
      cv::Point2f pt1 = curr_cam1_points[new_cam0_point.first] +
        Point2f(img_width, 0.0);

      circle(out_img, pt0, 3, new_feature, -1);
      circle(out_img, pt1, 3, new_feature, -1);
    }

    cv_bridge::CvImage debug_image(cam0_curr_img_ptr->header, "bgr8", out_img);
    debug_stereo_pub.publish(debug_image.toImageMsg());
  }
  //imshow("Feature", out_img);
  //waitKey(5);

  return;
}

void ImageProcessor::updateFeatureLifetime() {
  for (int code = 0; code <
      processor_config.grid_row*processor_config.grid_col; ++code) {
    vector<FeatureMetaData>& features = (*curr_features_ptr)[code];
    for (const auto& feature : features) {
      if (feature_lifetime.find(feature.id) == feature_lifetime.end())
        feature_lifetime[feature.id] = 1;
      else
        ++feature_lifetime[feature.id];
    }
  }

  return;
}

void ImageProcessor::featureLifetimeStatistics() {

  map<int, int> lifetime_statistics;
  for (const auto& data : feature_lifetime) {
    if (lifetime_statistics.find(data.second) ==
        lifetime_statistics.end())
      lifetime_statistics[data.second] = 1;
    else
      ++lifetime_statistics[data.second];
  }

  for (const auto& data : lifetime_statistics)
    cout << data.first << " : " << data.second << endl;

  return;
}

} // end namespace msckf_vio


#include <boost/math/common_factor.hpp>
#include <cmath>

#include "common/AlignmentUtils.h"
#include "core/Params.h"
#include "core/WorkerTracker.h"
#include "mapreduce/common/CoordinatorClientFactory.h"
#include "mapreduce/common/CoordinatorClientInterface.h"
#include "mapreduce/common/JobInfo.h"
#include "mapreduce/common/ReadRequest.h"
#include "mapreduce/common/Utils.h"

void loadReadRequests(
  WorkerTracker& readerTracker, const Params& params,
  const std::string& phaseName) {
  uint64_t numDisks = params.get<uint64_t>("NUM_INPUT_DISKS");
  for (uint64_t diskID = 0; diskID < numDisks; diskID++) {
    CoordinatorClientInterface* coordinatorClient =
      CoordinatorClientFactory::newCoordinatorClient(
        params, phaseName, "reader", diskID);
    ReadRequest* readRequest = NULL;
    do {
      readRequest = coordinatorClient->getNextReadRequest();
      // No matter whether this is NULL or not, add it to the tracker.
      readerTracker.addWorkUnit(readRequest);
      // But if the request was NULL, then we should stop after adding it.
    } while (readRequest != NULL);

    delete coordinatorClient;
  }
}

uint64_t setNumPartitions(
  uint64_t jobID, double intermediateToInputRatio, const Params& params) {
  // Get the input data size for the job.
  CoordinatorClientInterface* coordinatorClient =
    CoordinatorClientFactory::newCoordinatorClient(params, "", "", 0);
  JobInfo* jobInfo = coordinatorClient->getJobInfo(jobID);
  uint64_t totalInputSize = jobInfo->totalInputSize;
  delete jobInfo;

  // Compute intermediate data size
  uint64_t estimatedIntermediateDataSize =
    totalInputSize * intermediateToInputRatio;

  // Compute the number of partitions on each node and each disk
  uint64_t numNodes = params.get<uint64_t>("NUM_PEERS");
  uint64_t numIntermediateDisks =
    params.get<uint64_t>("NUM_OUTPUT_DISKS.phase_one");
  uint64_t numOutputDisks =
    params.get<uint64_t>("NUM_OUTPUT_DISKS.phase_two");
  uint64_t partitionSize = params.get<uint64_t>("PARTITION_SIZE");
  uint64_t partitionGroupsPerNode =
    params.get<uint64_t>("PARTITION_GROUPS_PER_NODE");

  uint64_t totalNumDisks = numNodes * numIntermediateDisks;
  uint64_t dataPerDisk = estimatedIntermediateDataSize / totalNumDisks;
  uint64_t partitionsPerDisk = std::max<uint64_t>(
      std::ceil(dataPerDisk / ((double) partitionSize)), 1);
  uint64_t partitionsPerNode = partitionsPerDisk * numIntermediateDisks;

  // Number of partitions per node must be a multiple of both the number of
  // disks and the number of partition groups per node, so round up.

  uint64_t partitionMultiple =
    boost::math::lcm(numIntermediateDisks, partitionGroupsPerNode);
  partitionMultiple = boost::math::lcm(numOutputDisks, partitionMultiple);
  partitionsPerNode = roundUp(partitionsPerNode, partitionMultiple);
  TRITONSORT_ASSERT(partitionsPerNode % numIntermediateDisks == 0,
         "Partitions per node %llu should be divisible by the number of "
         "intermediate disks %llu", partitionsPerNode, numIntermediateDisks);
  TRITONSORT_ASSERT(partitionsPerNode % numOutputDisks == 0,
         "Partitions per node %llu should be divisible by the number of "
         "output disks %llu", partitionsPerNode, numOutputDisks);
  TRITONSORT_ASSERT(partitionsPerNode % partitionGroupsPerNode == 0,
         "Partitions per node %llu should be divisible by the number of "
         "partition groups %llu", partitionsPerNode, partitionGroupsPerNode);

  // Finally compute the number of partitions for the job, and inform the
  // coordinator
  uint64_t numPartitions = partitionsPerNode * numNodes;
  coordinatorClient->setNumPartitions(jobID, numPartitions);

  delete coordinatorClient;

  return numPartitions;
}


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include <QMessageBox>

#include "mitkWorkbenchUtil.h"

#include "PerfusionDataSimulationView.h"

#include "itkUnaryFunctorImageFilter.h"
#include <boost/tokenizer.hpp>
#include "mitkImageCast.h"
#include "mitkImageTimeSelector.h"
#include "mitkITKImageImport.h"
#include "mitkGaussianNoiseFunctor.h"
#include "mitkTwoCompartmentExchangeModel.h"
#include "mitkTwoCompartmentExchangeModelParameterizer.h"
#include "mitkTwoCompartmentExchangeModelFactory.h"
#include "mitkNumericTwoCompartmentExchangeModel.h"
#include "mitkNumericTwoCompartmentExchangeModelParameterizer.h"
#include "mitkNumericTwoCompartmentExchangeModelFactory.h"
#include "mitkExtendedToftsModel.h"
#include "mitkExtendedToftsModelFactory.h"
#include "mitkExtendedToftsModelParameterizer.h"
#include "mitkTwoTissueCompartmentModel.h"
#include "mitkTwoTissueCompartmentModelParameterizer.h"
#include "mitkTwoTissueCompartmentModelFactory.h"
#include "mitkOneTissueCompartmentModel.h"
#include "mitkOneTissueCompartmentModelParameterizer.h"
#include "mitkOneTissueCompartmentModelFactory.h"
#include "mitkModelSignalImageGenerator.h"
#include <mitkNodePredicateAnd.h>
#include <mitkNodePredicateNot.h>
#include <mitkNodePredicateProperty.h>
#include <mitkNodePredicateDataType.h>
#include "mitkSimpleFunctorBase.h"
#include "mitkArbitraryTimeGeometry.h"
#include <iostream>
 #include <QFileDialog>

const std::string PerfusionDataSimulationView::VIEW_ID = "org.mitk.gui.qt.pharmacokinetics.simulation";

inline double convertToDouble(const std::string& data)
{
  std::istringstream stepStream(data);
  stepStream.imbue(std::locale("C"));
  double value = 0.0;

  if (!(stepStream >> value) || !(stepStream.eof()))
  {
    mitkThrow() << "Cannot convert string to double. String: " << data;
  }
  return value;
}

inline double FindeMaxConcentration(std::vector<double> Concentration)
{
    double maximum = Concentration[0];
    for (std::vector<double>::size_type i =0; i<Concentration.size(); ++i)
    {
        maximum = std::max(maximum, Concentration[i]);
    }
return maximum;
}

void PerfusionDataSimulationView::SetFocus()
{
    m_Controls.GenerateBtn->setFocus();
}

void PerfusionDataSimulationView::CreateQtPartControl(QWidget* parent)
{
	m_Controls.setupUi(parent);
    m_Controls.GenerateBtn->setEnabled(false);

    m_Controls.groupBox_TM->hide();
    m_Controls.groupBox_2CXM->hide();
    m_Controls.groupBox_2TCM->hide();
    m_Controls.groupBox_1TCM->hide();
    m_Controls.groupBox_CNR->hide();

    this->InitModelComboBox();
    /** #2 @todo Reduce code ?
     */

    m_Controls.comboBox_F->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_F->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_PS->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_PS->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_fp->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_fp->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_fi->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_fi->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_Ktrans->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_Ktrans->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_vp->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_vp->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_ve->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_ve->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_K1->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_K1->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_K2->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_K2->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_K3->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_K3->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_K4->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_K4->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_VB->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_VB->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_k1->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_k1->SetPredicate(m_IsNotABinaryImagePredicate);

    m_Controls.comboBox_k2->SetDataStorage(this->GetDataStorage());
    m_Controls.comboBox_k2->SetPredicate(m_IsNotABinaryImagePredicate);

    connect(m_Controls.AifFileBtn, SIGNAL(clicked()), this, SLOT(LoadAIFFile()));
    connect(m_Controls.ModelSelection, SIGNAL(currentIndexChanged(int)), this, SLOT(OnModellSet(int)));
    connect(m_Controls.GenerateBtn, SIGNAL(clicked()), this, SLOT(OnGenerateDataButtonClicked()));
    connect(m_Controls.comboBox_F, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_PS, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_fp, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_fi, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_VB, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_K1, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_K2, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_K3, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_K4, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_k1, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_k2, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_Ktrans, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_vp, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.comboBox_ve, SIGNAL(currentIndexChanged(int)), this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.CNRSpinBox, SIGNAL(valueChanged(double)),this, SLOT(OnSimulationConfigurationChanged()));
    connect(m_Controls.NoiseCheckBox, SIGNAL(stateChanged(int)),this, SLOT(OnSimulationConfigurationChanged()));

//    UpdateDataSelection();

  }

void PerfusionDataSimulationView::UpdateDataSelection()
{

}

//void PerfusionDataSimulationView::OnSelectionChanged(berry::IWorkbenchPart::Pointer /*source*/,
//    const QList<mitk::DataNode::Pointer>& selectedNodes)
//{
//    UpdateDataSelection();
//    m_Controls.GenerateBtn->setEnabled(CheckModelSettings());

//}

void PerfusionDataSimulationView::OnSimulationConfigurationChanged()
{
//    UpdateDataSelection();

    m_Controls.groupBox_CNR->setVisible(m_Controls.NoiseCheckBox->isChecked());

    this->m_CNR = m_Controls.CNRSpinBox->value();

    m_Controls.GenerateBtn->setEnabled( CheckModelSettings() );

}

void PerfusionDataSimulationView::OnModellSet(int index)
{
  m_selectedModelFactory = nullptr;

  if (index > 0)
  {
    if (static_cast<ModelFactoryStackType::size_type>(index) <= m_FactoryStack.size() )
    {
        m_selectedModelFactory = m_FactoryStack[index - 1];
    }
    else
    {
        MITK_WARN << "Invalid model index. Index outside of the factory stack. Factory stack size: "<< m_FactoryStack.size() << "; invalid index: "<< index;
    }
  }

  bool isToftsFactory = dynamic_cast<mitk::ExtendedToftsModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
  bool is2CXMFactory = dynamic_cast<mitk::TwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr || dynamic_cast<mitk::NumericTwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
  bool is2TCMFactory = dynamic_cast<mitk::TwoTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
  bool is1TCMFactory = dynamic_cast<mitk::OneTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;

  m_Controls.groupBox_TM->setVisible(isToftsFactory);
  m_Controls.groupBox_2CXM->setVisible(is2CXMFactory );
  m_Controls.groupBox_2TCM->setVisible(is2TCMFactory );
  m_Controls.groupBox_1TCM->setVisible(is1TCMFactory );

  m_Controls.GenerateBtn->setEnabled( CheckModelSettings() );
//  UpdateDataSelection();

}

bool PerfusionDataSimulationView::CheckModelSettings()
{
    bool ok = true;
    if(m_selectedModelFactory.IsNull())
    {
        return false;
    }

    if(this->m_AterialInputFunction.GetSize() == 0 || this->m_TimeGrid.GetSize() == 0)
    {
        return false;

    }

    bool isToftsFactory = dynamic_cast<mitk::ExtendedToftsModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool is2CXMFactory = dynamic_cast<mitk::TwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr || dynamic_cast<mitk::NumericTwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool is2TCMFactory = dynamic_cast<mitk::TwoTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool is1TCMFactory = dynamic_cast<mitk::OneTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;

    if(isToftsFactory)
    {
        ok = ok && m_Controls.comboBox_Ktrans->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_vp->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_ve->GetSelectedNode().IsNotNull();
    }
    else if(is2CXMFactory)
    {
        ok = ok && m_Controls.comboBox_F->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_PS->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_fp->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_fi->GetSelectedNode().IsNotNull();
    }

    else if(is2TCMFactory)
    {
        ok = ok && m_Controls.comboBox_K1->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_K2->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_K3->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_K4->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_VB->GetSelectedNode().IsNotNull();
    }
    else if(is1TCMFactory)
    {
        ok = ok && m_Controls.comboBox_k1->GetSelectedNode().IsNotNull();
        ok = ok && m_Controls.comboBox_k2->GetSelectedNode().IsNotNull();
     }

    else
    {
        return false;

    }
    if(m_Controls.NoiseCheckBox->isChecked())
    {
        if(m_CNR !=0 && m_MaxConcentration !=0)
        {
            this->m_Sigma = m_MaxConcentration/m_CNR;
        }
        if(m_Sigma==0)
        {
            return false;
        }
    }

    return ok;

}

void PerfusionDataSimulationView::InitModelComboBox() const
{
  this->m_Controls.ModelSelection->clear();
  this->m_Controls.ModelSelection->addItem(tr("No model selected"));

  for (ModelFactoryStackType::const_iterator pos = m_FactoryStack.begin(); pos != m_FactoryStack.end(); ++pos)
  {
    this->m_Controls.ModelSelection->addItem(QString::fromStdString((*pos)->GetClassID()));
  }
  this->m_Controls.ModelSelection->setCurrentIndex(0);
};

void PerfusionDataSimulationView::LoadAIFFile()
{
  QFileDialog dialog;
  dialog.setNameFilter(tr("Images (*.csv"));

  QString fileName = dialog.getOpenFileName();

  m_Controls.AifFilePath->setText( fileName );

  std::string aifFilePath = fileName.toStdString();

  //Read Input
  typedef boost::tokenizer< boost::escaped_list_separator<char> > Tokenizer;
  /////////////////////////////////////////////////////////////////////////////////////////////////
  //AIF Data

  std::vector<double> inputFunction;
  std::vector<double> inputGrid;

  std::ifstream in1(aifFilePath.c_str());

  if(!in1.is_open())
  {
      m_Controls.errorMessageLabel->setText("Could not open AIF File!");
  }

  std::vector< std::string > vec1;
  std::string line1;

  while (getline(in1,line1))
  {
      Tokenizer tok(line1);
      vec1.assign(tok.begin(),tok.end());

//        if (vec1.size() < 3) continue;

      inputGrid.push_back(convertToDouble(vec1[0]));
      inputFunction.push_back(convertToDouble(vec1[1]));

  }

  this->m_MaxConcentration = FindeMaxConcentration(inputFunction);

  itk::Array<double> aif;
  itk::Array<double> grid;

  aif.SetSize(inputFunction.size());
  grid.SetSize(inputGrid.size());

  aif.fill(0.0);
  grid.fill(0.0);

  itk::Array<double>::iterator aifPos = aif.begin();
  for(std::vector<double>::const_iterator pos = inputFunction.begin(); pos != inputFunction.end(); ++pos, ++aifPos)
  {
      *aifPos = *pos;
  }
  itk::Array<double>::iterator gridPos = grid.begin();
  for(std::vector<double>::const_iterator pos = inputGrid.begin(); pos != inputGrid.end(); ++pos, ++gridPos)
  {
      *gridPos = *pos;
  }

  this->m_AterialInputFunction = aif;
  this->m_TimeGrid = grid;

  m_Controls.GenerateBtn->setEnabled( CheckModelSettings() );
//  UpdateDataSelection();

}
/** @todo #2 Same function for Numeric and analytic version of FillParameterMap2CXM
 */
void PerfusionDataSimulationView::FillParameterMap2CXM()
{
    ParameterMapType stack;

    mitk::DataNode::Pointer m_selectedNode = m_Controls.comboBox_F->GetSelectedNode();
    mitk::Image::Pointer m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_F,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_PS->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_PS,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_fp->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_vp,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_fi->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_ve,m_selectedImage));

    this->m_ParameterImageMap = stack;

}

void PerfusionDataSimulationView::FillParameterMapNumeric2CXM()
{
    ParameterMapType stack;

    mitk::DataNode::Pointer m_selectedNode = m_Controls.comboBox_F->GetSelectedNode();
    mitk::Image::Pointer m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_F,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_PS->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_PS,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_fp->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_vp,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_fi->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_ve,m_selectedImage));

    this->m_ParameterImageMap = stack;

}

void PerfusionDataSimulationView::FillParameterMapETM()
{
    ParameterMapType stack;

    mitk::DataNode::Pointer m_selectedNode = m_Controls.comboBox_Ktrans->GetSelectedNode();
    mitk::Image::Pointer m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::ExtendedToftsModel::NAME_PARAMETER_Ktrans,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_vp->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::ExtendedToftsModel::NAME_PARAMETER_vp,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_ve->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::ExtendedToftsModel::NAME_PARAMETER_ve,m_selectedImage));

    this->m_ParameterImageMap = stack;

}

void PerfusionDataSimulationView::FillParameterMap2TCM()
{
    ParameterMapType stack;

    mitk::DataNode::Pointer m_selectedNode = m_Controls.comboBox_K1->GetSelectedNode();
    mitk::Image::Pointer m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoTissueCompartmentModel::NAME_PARAMETER_K1,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_K2->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k2,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_K3->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k3,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_K4->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k4,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_VB->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::TwoTissueCompartmentModel::NAME_PARAMETER_VB,m_selectedImage));

    this->m_ParameterImageMap = stack;

}

void PerfusionDataSimulationView::FillParameterMap1TCM()
{
    ParameterMapType stack;

    mitk::DataNode::Pointer m_selectedNode = m_Controls.comboBox_k1->GetSelectedNode();
    mitk::Image::Pointer m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::OneTissueCompartmentModel::NAME_PARAMETER_k1,m_selectedImage));

    m_selectedNode = m_Controls.comboBox_k2->GetSelectedNode();
    m_selectedImage = dynamic_cast<mitk::Image*>(m_selectedNode->GetData());
    stack.insert(std::make_pair(mitk::OneTissueCompartmentModel::NAME_PARAMETER_k2,m_selectedImage));

    this->m_ParameterImageMap = stack;

}

void PerfusionDataSimulationView::OnGenerateDataButtonClicked()
{
    mitk::Image::Pointer m_DynamicImage = mitk::Image::New();

    bool isToftsFactory = dynamic_cast<mitk::ExtendedToftsModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool isPhysBrixFactory = dynamic_cast<mitk::TwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool isNumPhysBrixFactory = dynamic_cast<mitk::NumericTwoCompartmentExchangeModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool is2TCMFactory = dynamic_cast<mitk::TwoTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;
    bool is1TCMFactory = dynamic_cast<mitk::OneTissueCompartmentModelFactory*>(m_selectedModelFactory.GetPointer()) != nullptr;

    if(isToftsFactory)
    {
       this->FillParameterMapETM();
        m_DynamicImage = this->GenerateETModelData();
    }

    if (isPhysBrixFactory)
    {
        this->FillParameterMap2CXM();
        m_DynamicImage = this->Generate2CXModelData();
    }

    if (isNumPhysBrixFactory)
    {
        this->FillParameterMapNumeric2CXM();
        m_DynamicImage = this->GenerateNumeric2CXModelData();
    }

    if (is2TCMFactory)
    {
        this->FillParameterMap2TCM();
        m_DynamicImage = this->Generate2TCModelData();
    }
    if (is1TCMFactory)
    {
        this->FillParameterMap1TCM();
        m_DynamicImage = this->Generate1TCModelData();
    }

    mitk::DataNode::Pointer resultNode = mitk::DataNode::New();
    std::string nameOfResultImage = "SimulationData_";
    nameOfResultImage.append(m_selectedModelFactory->GetModelDisplayName());
    resultNode->SetProperty("name", mitk::StringProperty::New(nameOfResultImage) );
    resultNode->SetData(m_DynamicImage); // set data of new node
    this->GetDataStorage()->Add(resultNode);

}

mitk::Image::Pointer PerfusionDataSimulationView::Generate2CXModelData()
{
mitk::ModelSignalImageGenerator::Pointer generator = mitk::ModelSignalImageGenerator::New();
mitk::TwoCompartmentExchangeModelParameterizer::Pointer modelParameterizer = mitk::TwoCompartmentExchangeModelParameterizer::New();

/** @todo #2  necessary? Generator need to have a map with Parameters in order of Model in order to pass vector parameters correctly to Model.
 * I wanted to make it independend from the order the images are passed on
 */
for(ParameterMapType::const_iterator pos = this->m_ParameterImageMap.begin(); pos != this->m_ParameterImageMap.end(); ++pos)
{
    if(pos->first == mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_F)
    {
        generator->SetParameterInputImage(mitk::TwoCompartmentExchangeModel::POSITION_PARAMETER_F, pos->second);
    }
    if(pos->first == mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_PS)
    {
        generator->SetParameterInputImage(mitk::TwoCompartmentExchangeModel::POSITION_PARAMETER_PS, pos->second);
    }
    if(pos->first == mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_ve)
    {
        generator->SetParameterInputImage(mitk::TwoCompartmentExchangeModel::POSITION_PARAMETER_ve, pos->second);
    }
    if(pos->first == mitk::TwoCompartmentExchangeModel::NAME_PARAMETER_vp)
    {
        generator->SetParameterInputImage(mitk::TwoCompartmentExchangeModel::POSITION_PARAMETER_vp, pos->second);
    }
}

modelParameterizer->SetAIF(this->m_AterialInputFunction);
modelParameterizer->SetAIFTimeGrid(this->m_TimeGrid);
modelParameterizer->SetDefaultTimeGrid(this->m_TimeGrid);

generator->SetParameterizer(modelParameterizer);

mitk::Image::Pointer generatedImage = generator->GetGeneratedImage();

 mitk::Image::Pointer resultImage = mitk::Image::New();
if(m_Controls.NoiseCheckBox->isChecked())
{
    typedef itk::Image<double,3> ImageType;

    mitk::Image::Pointer tempImage = mitk::Image::New();
    tempImage->Initialize(generatedImage);

    mitk::ArbitraryTimeGeometry* timeGeometry = dynamic_cast<mitk::ArbitraryTimeGeometry*> (generatedImage->GetTimeGeometry());

    tempImage->SetTimeGeometry(timeGeometry);

    ImageType::Pointer itkImage = ImageType::New();
    mitk::ImageTimeSelector::Pointer imageTimeSelector = mitk::ImageTimeSelector::New();
    imageTimeSelector->SetInput(generatedImage);

    for(unsigned int i = 0; i< generatedImage->GetTimeSteps(); ++i)
    {
        imageTimeSelector->SetTimeNr(i);
        imageTimeSelector->UpdateLargestPossibleRegion();

        mitk::Image::Pointer mitkInputImage = imageTimeSelector->GetOutput();

        mitk::CastToItkImage(mitkInputImage, itkImage );

        typedef mitk::GaussianNoiseFunctor<double,double> NoiseFunctorType;
        typedef itk::UnaryFunctorImageFilter<ImageType,ImageType, NoiseFunctorType > NoiseFilterType;
        NoiseFilterType::Pointer noiseFilter = NoiseFilterType::New();
        NoiseFunctorType noiseFunctor;
        noiseFunctor.SetMean(0.0);
        noiseFunctor.SetSigma(this->m_Sigma);
        noiseFilter->SetFunctor(noiseFunctor);

        noiseFilter->SetInput(itkImage);

        mitk::Image::Pointer outputImage = mitk::ImportItkImage(noiseFilter->GetOutput())->Clone();

        mitk::ImageReadAccessor accessor(outputImage);
        tempImage->SetVolume(accessor.GetData(), i);
    }

   resultImage = tempImage->Clone();
}
else
{
    resultImage = generatedImage;
}
return resultImage;
}

/** @todo #2  Synergie? Function implementation for every Model to complicated?
 */
mitk::Image::Pointer PerfusionDataSimulationView::GenerateNumeric2CXModelData()
{
mitk::ModelSignalImageGenerator::Pointer generator = mitk::ModelSignalImageGenerator::New();
mitk::NumericTwoCompartmentExchangeModelParameterizer::Pointer modelParameterizer = mitk::NumericTwoCompartmentExchangeModelParameterizer::New();

for(ParameterMapType::const_iterator pos = this->m_ParameterImageMap.begin(); pos != this->m_ParameterImageMap.end(); ++pos)
{
    if(pos->first == mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_F)
    {
        generator->SetParameterInputImage(mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_F, pos->second);
    }
    if(pos->first == mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_PS)
    {
        generator->SetParameterInputImage(mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_PS, pos->second);
    }
    if(pos->first == mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_ve)
    {
        generator->SetParameterInputImage(mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_ve, pos->second);
    }
    if(pos->first == mitk::NumericTwoCompartmentExchangeModel::NAME_PARAMETER_vp)
    {
        generator->SetParameterInputImage(mitk::NumericTwoCompartmentExchangeModel::POSITION_PARAMETER_vp, pos->second);
    }
}

modelParameterizer->SetAIF(this->m_AterialInputFunction);
modelParameterizer->SetAIFTimeGrid(this->m_TimeGrid);
modelParameterizer->SetDefaultTimeGrid(this->m_TimeGrid);
modelParameterizer->SetODEINTStepSize(0.05);

generator->SetParameterizer(modelParameterizer);

mitk::Image::Pointer generatedImage = generator->GetGeneratedImage();

mitk::Image::Pointer resultImage = mitk::Image::New();
if(m_Controls.NoiseCheckBox->isChecked())
{
   typedef itk::Image<double,3> ImageType;

   mitk::Image::Pointer tempImage = mitk::Image::New();
   tempImage->Initialize(generatedImage);

   mitk::ArbitraryTimeGeometry* timeGeometry = dynamic_cast<mitk::ArbitraryTimeGeometry*> (generatedImage->GetTimeGeometry());

   tempImage->SetTimeGeometry(timeGeometry);

   ImageType::Pointer itkImage = ImageType::New();
   mitk::ImageTimeSelector::Pointer imageTimeSelector = mitk::ImageTimeSelector::New();
   imageTimeSelector->SetInput(generatedImage);

   for(unsigned int i = 0; i< generatedImage->GetTimeSteps(); ++i)
   {
       imageTimeSelector->SetTimeNr(i);
       imageTimeSelector->UpdateLargestPossibleRegion();

       mitk::Image::Pointer mitkInputImage = imageTimeSelector->GetOutput();

       mitk::CastToItkImage(mitkInputImage, itkImage );

       typedef mitk::GaussianNoiseFunctor<double,double> NoiseFunctorType;
       typedef itk::UnaryFunctorImageFilter<ImageType,ImageType, NoiseFunctorType > NoiseFilterType;
       NoiseFilterType::Pointer noiseFilter = NoiseFilterType::New();
       NoiseFunctorType noiseFunctor;
       noiseFunctor.SetMean(0.0);
       noiseFunctor.SetSigma(this->m_Sigma);
       noiseFilter->SetFunctor(noiseFunctor);

       noiseFilter->SetInput(itkImage);

       mitk::Image::Pointer outputImage = mitk::ImportItkImage(noiseFilter->GetOutput())->Clone();

       mitk::ImageReadAccessor accessor(outputImage);
       tempImage->SetVolume(accessor.GetData(), i);
   }

  resultImage = tempImage->Clone();
}
else
{
   resultImage = generatedImage;
}
return resultImage;
}

mitk::Image::Pointer PerfusionDataSimulationView::GenerateETModelData()
{
mitk::ModelSignalImageGenerator::Pointer generator = mitk::ModelSignalImageGenerator::New();
mitk::ExtendedToftsModelParameterizer::Pointer modelParameterizer = mitk::ExtendedToftsModelParameterizer::New();

for(ParameterMapType::const_iterator pos = this->m_ParameterImageMap.begin(); pos != this->m_ParameterImageMap.end(); ++pos)
{
    if(pos->first == mitk::ExtendedToftsModel::NAME_PARAMETER_Ktrans)
    {
        generator->SetParameterInputImage(mitk::ExtendedToftsModel::POSITION_PARAMETER_Ktrans, pos->second);
    }
    if(pos->first == mitk::ExtendedToftsModel::NAME_PARAMETER_vp)
    {
        generator->SetParameterInputImage(mitk::ExtendedToftsModel::POSITION_PARAMETER_vp, pos->second);
    }
    if(pos->first == mitk::ExtendedToftsModel::NAME_PARAMETER_ve)
    {
        generator->SetParameterInputImage(mitk::ExtendedToftsModel::POSITION_PARAMETER_ve, pos->second);
    }

}

modelParameterizer->SetAIF(this->m_AterialInputFunction);
modelParameterizer->SetAIFTimeGrid(this->m_TimeGrid);
modelParameterizer->SetDefaultTimeGrid(this->m_TimeGrid);

generator->SetParameterizer(modelParameterizer);

mitk::Image::Pointer generatedImage = generator->GetGeneratedImage();

mitk::Image::Pointer resultImage = mitk::Image::New();
if(m_Controls.NoiseCheckBox->isChecked())
{
   typedef itk::Image<double,3> ImageType;

   mitk::Image::Pointer tempImage = mitk::Image::New();
   tempImage->Initialize(generatedImage);

   mitk::ArbitraryTimeGeometry* timeGeometry = dynamic_cast<mitk::ArbitraryTimeGeometry*> (generatedImage->GetTimeGeometry());

   tempImage->SetTimeGeometry(timeGeometry);

   ImageType::Pointer itkImage = ImageType::New();
   mitk::ImageTimeSelector::Pointer imageTimeSelector = mitk::ImageTimeSelector::New();
   imageTimeSelector->SetInput(generatedImage);

   for(unsigned int i = 0; i< generatedImage->GetTimeSteps(); ++i)
   {
       imageTimeSelector->SetTimeNr(i);
       imageTimeSelector->UpdateLargestPossibleRegion();

       mitk::Image::Pointer mitkInputImage = imageTimeSelector->GetOutput();

       mitk::CastToItkImage(mitkInputImage, itkImage );

       typedef mitk::GaussianNoiseFunctor<double,double> NoiseFunctorType;
       typedef itk::UnaryFunctorImageFilter<ImageType,ImageType, NoiseFunctorType > NoiseFilterType;
       NoiseFilterType::Pointer noiseFilter = NoiseFilterType::New();
       NoiseFunctorType noiseFunctor;
       noiseFunctor.SetMean(0.0);
       noiseFunctor.SetSigma(this->m_Sigma);
       noiseFilter->SetFunctor(noiseFunctor);

       noiseFilter->SetInput(itkImage);

       mitk::Image::Pointer outputImage = mitk::ImportItkImage(noiseFilter->GetOutput())->Clone();

       mitk::ImageReadAccessor accessor(outputImage);
       tempImage->SetVolume(accessor.GetData(), i);
   }

  resultImage = tempImage->Clone();
}
else
{
   resultImage = generatedImage;
}
return resultImage;
}

mitk::Image::Pointer PerfusionDataSimulationView::Generate2TCModelData()
{
mitk::ModelSignalImageGenerator::Pointer generator = mitk::ModelSignalImageGenerator::New();
mitk::TwoTissueCompartmentModelParameterizer::Pointer modelParameterizer = mitk::TwoTissueCompartmentModelParameterizer::New();

for(ParameterMapType::const_iterator pos = this->m_ParameterImageMap.begin(); pos != this->m_ParameterImageMap.end(); ++pos)
{
    if(pos->first == mitk::TwoTissueCompartmentModel::NAME_PARAMETER_K1)
    {
        generator->SetParameterInputImage(mitk::TwoTissueCompartmentModel::POSITION_PARAMETER_K1, pos->second);
    }
    if(pos->first == mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k2)
    {
        generator->SetParameterInputImage(mitk::TwoTissueCompartmentModel::POSITION_PARAMETER_k2, pos->second);
    }
    if(pos->first == mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k3)
    {
        generator->SetParameterInputImage(mitk::TwoTissueCompartmentModel::POSITION_PARAMETER_k3, pos->second);
    }
    if(pos->first == mitk::TwoTissueCompartmentModel::NAME_PARAMETER_k4)
    {
        generator->SetParameterInputImage(mitk::TwoTissueCompartmentModel::POSITION_PARAMETER_k4, pos->second);
    }
    if(pos->first == mitk::TwoTissueCompartmentModel::NAME_PARAMETER_VB)
    {
        generator->SetParameterInputImage(mitk::TwoTissueCompartmentModel::POSITION_PARAMETER_VB, pos->second);
    }

}

modelParameterizer->SetAIF(this->m_AterialInputFunction);
modelParameterizer->SetAIFTimeGrid(this->m_TimeGrid);
modelParameterizer->SetDefaultTimeGrid(this->m_TimeGrid);

generator->SetParameterizer(modelParameterizer);

mitk::Image::Pointer generatedImage = generator->GetGeneratedImage();

 mitk::Image::Pointer resultImage = mitk::Image::New();
if(m_Controls.NoiseCheckBox->isChecked())
{
    typedef itk::Image<double,3> ImageType;

    mitk::Image::Pointer tempImage = mitk::Image::New();
    tempImage->Initialize(generatedImage);

    mitk::ArbitraryTimeGeometry* timeGeometry = dynamic_cast<mitk::ArbitraryTimeGeometry*> (generatedImage->GetTimeGeometry());

    tempImage->SetTimeGeometry(timeGeometry);

    ImageType::Pointer itkImage = ImageType::New();
    mitk::ImageTimeSelector::Pointer imageTimeSelector = mitk::ImageTimeSelector::New();
    imageTimeSelector->SetInput(generatedImage);

    for(unsigned int i = 0; i< generatedImage->GetTimeSteps(); ++i)
    {
        imageTimeSelector->SetTimeNr(i);
        imageTimeSelector->UpdateLargestPossibleRegion();

        mitk::Image::Pointer mitkInputImage = imageTimeSelector->GetOutput();

        mitk::CastToItkImage(mitkInputImage, itkImage );

        typedef mitk::GaussianNoiseFunctor<double,double> NoiseFunctorType;
        typedef itk::UnaryFunctorImageFilter<ImageType,ImageType, NoiseFunctorType > NoiseFilterType;
        NoiseFilterType::Pointer noiseFilter = NoiseFilterType::New();
        NoiseFunctorType noiseFunctor;
        noiseFunctor.SetMean(0.0);
        noiseFunctor.SetSigma(this->m_Sigma);
        noiseFilter->SetFunctor(noiseFunctor);

        noiseFilter->SetInput(itkImage);

        mitk::Image::Pointer outputImage = mitk::ImportItkImage(noiseFilter->GetOutput())->Clone();

        mitk::ImageReadAccessor accessor(outputImage);
        tempImage->SetVolume(accessor.GetData(), i);
    }

   resultImage = tempImage->Clone();
}
else
{
    resultImage = generatedImage;
}
return resultImage;
}

mitk::Image::Pointer PerfusionDataSimulationView::Generate1TCModelData()
{
mitk::ModelSignalImageGenerator::Pointer generator = mitk::ModelSignalImageGenerator::New();
mitk::OneTissueCompartmentModelParameterizer::Pointer modelParameterizer = mitk::OneTissueCompartmentModelParameterizer::New();

for(ParameterMapType::const_iterator pos = this->m_ParameterImageMap.begin(); pos != this->m_ParameterImageMap.end(); ++pos)
{
    if(pos->first == mitk::OneTissueCompartmentModel::NAME_PARAMETER_k1)
    {
        generator->SetParameterInputImage(mitk::OneTissueCompartmentModel::POSITION_PARAMETER_k1, pos->second);
    }
    if(pos->first == mitk::OneTissueCompartmentModel::NAME_PARAMETER_k2)
    {
        generator->SetParameterInputImage(mitk::OneTissueCompartmentModel::POSITION_PARAMETER_k2, pos->second);
    }

}

modelParameterizer->SetAIF(this->m_AterialInputFunction);
modelParameterizer->SetAIFTimeGrid(this->m_TimeGrid);
modelParameterizer->SetDefaultTimeGrid(this->m_TimeGrid);

generator->SetParameterizer(modelParameterizer);

mitk::Image::Pointer generatedImage = generator->GetGeneratedImage();

 mitk::Image::Pointer resultImage = mitk::Image::New();
if(m_Controls.NoiseCheckBox->isChecked())
{
    typedef itk::Image<double,3> ImageType;

    mitk::Image::Pointer tempImage = mitk::Image::New();
    tempImage->Initialize(generatedImage);

    mitk::ArbitraryTimeGeometry* timeGeometry = dynamic_cast<mitk::ArbitraryTimeGeometry*> (generatedImage->GetTimeGeometry());

    tempImage->SetTimeGeometry(timeGeometry);

    ImageType::Pointer itkImage = ImageType::New();
    mitk::ImageTimeSelector::Pointer imageTimeSelector = mitk::ImageTimeSelector::New();
    imageTimeSelector->SetInput(generatedImage);

    for(unsigned int i = 0; i< generatedImage->GetTimeSteps(); ++i)
    {
        imageTimeSelector->SetTimeNr(i);
        imageTimeSelector->UpdateLargestPossibleRegion();

        mitk::Image::Pointer mitkInputImage = imageTimeSelector->GetOutput();

        mitk::CastToItkImage(mitkInputImage, itkImage );

        typedef mitk::GaussianNoiseFunctor<double,double> NoiseFunctorType;
        typedef itk::UnaryFunctorImageFilter<ImageType,ImageType, NoiseFunctorType > NoiseFilterType;
        NoiseFilterType::Pointer noiseFilter = NoiseFilterType::New();
        NoiseFunctorType noiseFunctor;
        noiseFunctor.SetMean(0.0);
        noiseFunctor.SetSigma(this->m_Sigma);
        noiseFilter->SetFunctor(noiseFunctor);

        noiseFilter->SetInput(itkImage);

        mitk::Image::Pointer outputImage = mitk::ImportItkImage(noiseFilter->GetOutput())->Clone();

        mitk::ImageReadAccessor accessor(outputImage);
        tempImage->SetVolume(accessor.GetData(), i);
    }

   resultImage = tempImage->Clone();
}
else
{
    resultImage = generatedImage;
}
return resultImage;
}

PerfusionDataSimulationView::PerfusionDataSimulationView()
{
     m_Sigma = 0;
     m_CNR=0;
     m_MaxConcentration=0;
     m_selectedModelFactory = nullptr;

     mitk::ModelFactoryBase::Pointer factory = mitk::ExtendedToftsModelFactory::New().GetPointer();
     m_FactoryStack.push_back(factory);
     factory = mitk::TwoCompartmentExchangeModelFactory::New().GetPointer();
     m_FactoryStack.push_back(factory);
     factory = mitk::NumericTwoCompartmentExchangeModelFactory::New().GetPointer();
     m_FactoryStack.push_back(factory);
     factory = mitk::TwoTissueCompartmentModelFactory::New().GetPointer();
     m_FactoryStack.push_back(factory);
     factory = mitk::OneTissueCompartmentModelFactory::New().GetPointer();
     m_FactoryStack.push_back(factory);

     m_IsNotABinaryImagePredicate = mitk::NodePredicateAnd::New(
                 mitk::TNodePredicateDataType<mitk::Image>::New(),
                 mitk::NodePredicateNot::New(mitk::NodePredicateProperty::New("binary", mitk::BoolProperty::New(true))),
                 mitk::NodePredicateNot::New(mitk::NodePredicateProperty::New("helper object"))).GetPointer();

}