<commit_before>#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>
#include <pthread.h>
#include <geometry_msgs/Point.h>

static const std::string RGB_WINDOW_NAME = "RGB image";
static const std::string DEPTH_WINDOW_NAME = "Depth image";

class SearchTomato {
public:
  SearchTomato()
    : found_flag(false)
    , tick_count(0)
    , last_tick_count(0)
    , not_founding_count(0)
    , kf(6, 4, 0, CV_32F)
    , state(6, 1, CV_32F)
  {
    init_set_kalman(kf);
  }

  ~SearchTomato() {}

  bool searchTomatoPoint(const cv::Mat& capture_rgb, cv::Point& tomato_point) {
    cv::Mat binary_mat = cv::Mat::zeros(capture_rgb.size(), CV_8UC1);
    std::vector<cv::Rect> tomato_boxs;
    // reset transition matrix of kalman filter.
    tick_count = cv::getTickCount();
    double d_time = (tick_count - last_tick_count) / cv::getTickFrequency(); // d_time: infinitesimal difference of time.
    kf.transitionMatrix.at<float>(2) = d_time; // x = x + dt*vx
    kf.transitionMatrix.at<float>(9) = d_time; // y = y + dt*vy
    last_tick_count = tick_count;

    // image processing and seach box for tomato
    imageProcessing(capture_rgb, binary_mat);
    imshow(DEPTH_WINDOW_NAME, binary_mat);
    searchTomatoBox(binary_mat, tomato_boxs);
    printf("search box num is %lu", tomato_boxs.size());

    if (tomato_boxs.size() > 0) {
      not_founding_count = 0;
      if (!found_flag) {
	resetKalman();
	found_flag = true;
      } else
	applyKalman(tomato_boxs);

    } else
      if (++not_founding_count > 10) {
	not_founding_count = 10;
	found_flag = false;
      }

    // set point of tomato
    state = kf.predict();
    tomato_point.x = state.at<float>(0);
    tomato_point.y = state.at<float>(1);

    return found_flag;
  }

private:
  void init_set_kalman(cv::KalmanFilter& kf) {
    // transition state matrix(A). think for [x, y, vx, vy, width, height]. identiry mean state is static.
    cv::setIdentity(kf.transitionMatrix); // NOTE: set state of speed at each step!

    // measurement matrix(H). mean look to only [x, y, width, height].
    kf.measurementMatrix = cv::Mat::zeros(4, 6, CV_32F);
    kf.measurementMatrix.at<float>(0) = 1.0f;
    kf.measurementMatrix.at<float>(7) = 1.0f;
    kf.measurementMatrix.at<float>(16) = 1.0f;
    kf.measurementMatrix.at<float>(23) = 1.0f;

    // process noise covariance matrix(Q). all state have noise is 5e-2;
    cv::setIdentity(kf.processNoiseCov, cv::Scalar(5e-2));
  }

  void imageProcessing(const cv::Mat& rgb, cv::Mat& binary_mat) {
    cv::Mat blur, hsv;
    cv::GaussianBlur(rgb, blur, cv::Size(5, 5), 3.0, 3.0);
    cv::cvtColor(blur, hsv, CV_BGR2HSV);
    redFilter(hsv, binary_mat);
    cv::erode(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 3);
    cv::dilate(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 5);
    cv::erode(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 3);
  }

  void redFilter(cv::Mat& hsv, cv::Mat& binary_mat) {
    int a, x, y;
    for(y = 0; y < hsv.rows; y++) {
      for(x = 0; x < hsv.cols; x++) {
	a = hsv.step*y+(x*3);
	if((hsv.data[a] <= 5 || hsv.data[a] >= 175) && hsv.data[a+1] >= 50 && hsv.data[a+2] >= 50 )
	  binary_mat.at<unsigned char>(y,x) = 255;
	else
	  binary_mat.at<unsigned char>(y,x) = 0;
      }
    }
  }

  void searchTomatoBox(cv::Mat& binary_mat, std::vector<cv::Rect>& tomato_boxs) {
    std::vector<std::vector<cv::Point> > contours;
    cv::findContours(binary_mat, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
    findMatchTomato(contours, tomato_boxs);
  }

  void findMatchTomato(std::vector<std::vector<cv::Point> >& contours, std::vector<cv::Rect>& tomato_boxs) {
    cv::Rect bounding_box;
    for (unsigned int i = 0; i < contours.size(); i++) {
      bounding_box = cv::boundingRect(contours[i]);

      float ratio_balance = (float)bounding_box.width / (float)bounding_box.height;
	if (ratio_balance > 1.0f) ratio_balance = 1.0f / ratio_balance;

      // delete mismach. that is smaller or spier
      if (bounding_box.area() >= 500 && bounding_box.area() < 2*cv::contourArea(contours[i]) && ratio_balance > 0.4f)
	tomato_boxs.push_back(bounding_box);
    }
  }

  void applyKalman(std::vector<cv::Rect>& tomato_boxs) {
    cv::Mat meas(4, 1, CV_32F);
    int i = findIndexPlausible(tomato_boxs);

    meas.at<float>(0) = tomato_boxs[i].x + tomato_boxs[i].width/2;
    meas.at<float>(1) = tomato_boxs[i].y + tomato_boxs[i].height/2;
    meas.at<float>(2) = tomato_boxs[i].width;
    meas.at<float>(3) = tomato_boxs[i].height;
    kf.correct(meas);
  }

  int findIndexPlausible(std::vector<cv::Rect>& tomato_boxs) {
    int index = 0;
    unsigned int worst_non_likelihood = 0xffff;
    unsigned int non_likelihood;
    for (int i = 0; i < tomato_boxs.size(); i++) {
      state = kf.predict();
      non_likelihood = 0;
      non_likelihood += abs(tomato_boxs[i].x - state.at<float>(0));
      non_likelihood += abs(tomato_boxs[i].y - state.at<float>(1));
      non_likelihood += abs(tomato_boxs[i].width - state.at<float>(4));
      non_likelihood += abs(tomato_boxs[i].height - state.at<float>(5));
      if (worst_non_likelihood > non_likelihood) {
	worst_non_likelihood = non_likelihood;
	index = i;
      }
    }
    return index;
  }

  void resetKalman() {
    kf.errorCovPre.at<float>(0) = 1; // px
    kf.errorCovPre.at<float>(7) = 1; // px
    kf.errorCovPre.at<float>(14) = 1;
    kf.errorCovPre.at<float>(21) = 1;
    kf.errorCovPre.at<float>(28) = 1; // px
    kf.errorCovPre.at<float>(35) = 1; // px
  }

  bool found_flag;
  long tick_count, last_tick_count; // count while running.
  unsigned int not_founding_count;
  cv::KalmanFilter kf;
  cv::Mat state;
};

class ImageConverter {
private:
  ros::NodeHandle nh;
  ros::Publisher point_pub;
  image_transport::ImageTransport it;
  image_transport::Subscriber rgb_sub;
  image_transport::Subscriber depth_sub;
  cv_bridge::CvImageConstPtr rgb_ptr;
  cv_bridge::CvImageConstPtr depth_ptr;
  SearchTomato searchTomatoObj;
  cv::Point tomato_point;
  bool found_flag;
  geometry_msgs::Point pub_msg;

public:
  ImageConverter()
    : it(nh)
    , searchTomatoObj()
    , tomato_point(-1, -1)
    , found_flag(false)
  {
    // subscrive to input video feed.
    point_pub = nh.advertise<geometry_msgs::Point>("tomato_point", 1);
    rgb_sub = it.subscribe("/camera/rgb/image_raw", 1, &ImageConverter::rgbCb, this);
    depth_sub = it.subscribe("/camera/depth_registered/image_raw", 1, &ImageConverter::depthCb, this);
  }

  ~ImageConverter() {}

  /**
   * search tomato function.
   * get rgb image, and search tomato.
   * will tomato_point have data.
   *
   * @author Yusuke Doi
   */
  void rgbCb(const sensor_msgs::ImageConstPtr& msg) {
    // get rgb image on kinect
    try {
      rgb_ptr = cv_bridge::toCvShare(msg, "bgr8");
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception by rgb: %s", e.what());
    }

    cv::Mat buf = rgb_ptr->image.clone();
    // search tomato
    if (found_flag = searchTomatoObj.searchTomatoPoint(rgb_ptr->image, tomato_point))
      cv::circle(buf, tomato_point, 2, CV_RGB(0, 255, 0), -1);

    cv::imshow(RGB_WINDOW_NAME, buf);
    cv::waitKey(100);
  }

  void depthCb(const sensor_msgs::ImageConstPtr& msg) {
    try {
      depth_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_16UC1);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception by depth: %s", e.what());
    }

    //testShowDepth(depth_ptr->image);
    if (found_flag) {
      printf("depth is %u", depth_ptr->image.at<uint16_t>(tomato_point));
//      pub_msg.x = tomato_point.x;
//      pub_msg.y = tomato_point.y;
//      pub_msg.z = depth_ptr->image.at<uint16_t>(tomato_point);
      pub_msg.x = depth_ptr->image.at<uint16_t>(tomato_point);
      pub_msg.y = tomato_point.x;
      pub_msg.z = -tomato_point.y;
      point_pub.publish(pub_msg);
    }
    cv::waitKey(100);
  }

  void testShowDepth(const cv::Mat& depth) {
    static std::vector<uint16_t> m_gamma(2048);
    static bool firstFlag(false);
    if (!firstFlag) {
      for(unsigned int i = 0; i < 2048; i++) {
	float v = i/2048.0;
	v = std::pow(v, 3) * 6;
	m_gamma[i] = v*6*256;
      }
      firstFlag = true;
    }

    int i;
    cv::Mat buf = cv::Mat::zeros(depth.size(), CV_16UC1);
    cv::Mat output = cv::Mat::zeros(depth.size(), CV_8UC3);
    cv::normalize(depth, buf, 0, 2013, cv::NORM_MINMAX);

    for (i = 0; i < depth.rows * depth.cols; i++) {
      int y = (int)(i / depth.cols);
      int x = (int)(i % depth.cols);
      int pval = m_gamma[buf.at<uint16_t>(i)];
      int lb = pval & 0xff;

      switch(pval >> 8) {
      case 0:
	output.at<cv::Vec3b>(y,x)[2] = 255;
	output.at<cv::Vec3b>(y,x)[1] = 255-lb;
	output.at<cv::Vec3b>(y,x)[0] = 255-lb;
	break;
      case 1:
	output.at<cv::Vec3b>(y,x)[2] = 255;
	output.at<cv::Vec3b>(y,x)[1] = lb;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      case 2:
	output.at<cv::Vec3b>(y,x)[2] = 255-lb;
	output.at<cv::Vec3b>(y,x)[1] = 255;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      case 3:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 255;
	output.at<cv::Vec3b>(y,x)[0] = lb;
	break;
      case 4:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 255-lb;
	output.at<cv::Vec3b>(y,x)[0] = 255;
	break;
      case 5:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 0;
	output.at<cv::Vec3b>(y,x)[0] = 255-lb;
        break;
      default:
        output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 0;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      }
    }
    imshow(DEPTH_WINDOW_NAME, output);
  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "kinect_tomato_search_node");
  ImageConverter ic;
  cv::namedWindow(RGB_WINDOW_NAME);
  cv::namedWindow(DEPTH_WINDOW_NAME);
  ros::spin();
  cv::destroyWindow(RGB_WINDOW_NAME);
  cv::destroyWindow(DEPTH_WINDOW_NAME);
  return 0;
}
<commit_msg>Fix verbose code @kinect_tomato_searcher<commit_after>#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>
#include <pthread.h>
#include <geometry_msgs/Point.h>

static const std::string RGB_WINDOW_NAME = "RGB image";
static const std::string DEPTH_WINDOW_NAME = "Depth image";

class SearchTomato {
public:
  SearchTomato()
    : found_flag(false)
    , tick_count(0)
    , last_tick_count(0)
    , not_founding_count(0)
    , kf(6, 4, 0, CV_32F)
    , state(6, 1, CV_32F)
  {
    init_set_kalman(kf);
  }

  ~SearchTomato() {}

  bool searchTomatoPoint(const cv::Mat& capture_rgb, cv::Point& tomato_point) {
    cv::Mat binary_mat = cv::Mat::zeros(capture_rgb.size(), CV_8UC1);
    std::vector<cv::Rect> tomato_boxs;
    // reset transition matrix of kalman filter.
    tick_count = cv::getTickCount();
    double d_time = (tick_count - last_tick_count) / cv::getTickFrequency(); // d_time: infinitesimal difference of time.
    kf.transitionMatrix.at<float>(2) = d_time; // x = x + dt*vx
    kf.transitionMatrix.at<float>(9) = d_time; // y = y + dt*vy
    last_tick_count = tick_count;

    // image processing and seach box for tomato
    imageProcessing(capture_rgb, binary_mat);
    imshow(DEPTH_WINDOW_NAME, binary_mat);
    searchTomatoBox(binary_mat, tomato_boxs);
    printf("search box num is %lu", tomato_boxs.size());

    if (tomato_boxs.size() > 0) {
      not_founding_count = 0;
      if (!found_flag) {
	resetKalman();
	found_flag = true;
      } else
	applyKalman(tomato_boxs);

    } else
      if (++not_founding_count > 10) {
	not_founding_count = 10;
	found_flag = false;
      }

    // set point of tomato
    state = kf.predict();
    tomato_point.x = state.at<float>(0);
    tomato_point.y = state.at<float>(1);

    return found_flag;
  }

private:
  void init_set_kalman(cv::KalmanFilter& kf) {
    // transition state matrix(A). think for [x, y, vx, vy, width, height]. identiry mean state is static.
    cv::setIdentity(kf.transitionMatrix); // NOTE: set state of speed at each step!

    // measurement matrix(H). mean look to only [x, y, width, height].
    kf.measurementMatrix = cv::Mat::zeros(4, 6, CV_32F);
    kf.measurementMatrix.at<float>(0) = 1.0f;
    kf.measurementMatrix.at<float>(7) = 1.0f;
    kf.measurementMatrix.at<float>(16) = 1.0f;
    kf.measurementMatrix.at<float>(23) = 1.0f;

    // process noise covariance matrix(Q). all state have noise is 5e-2;
    cv::setIdentity(kf.processNoiseCov, cv::Scalar(5e-2));
  }

  void imageProcessing(const cv::Mat& rgb, cv::Mat& binary_mat) {
    cv::Mat blur, hsv;
    cv::GaussianBlur(rgb, blur, cv::Size(5, 5), 3.0, 3.0);
    cv::cvtColor(blur, hsv, CV_BGR2HSV);
    redFilter(hsv, binary_mat);
    cv::erode(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 3);
    cv::dilate(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 5);
    cv::erode(binary_mat, binary_mat, cv::Mat(), cv::Point(-1, -1), 3);
  }

  void redFilter(cv::Mat& hsv, cv::Mat& binary_mat) {
    int a, x, y;
    for(y = 0; y < hsv.rows; y++) {
      for(x = 0; x < hsv.cols; x++) {
	a = hsv.step*y+(x*3);
	if((hsv.data[a] <= 5 || hsv.data[a] >= 175) && hsv.data[a+1] >= 50 && hsv.data[a+2] >= 50 )
	  binary_mat.at<unsigned char>(y,x) = 255;
	else
	  binary_mat.at<unsigned char>(y,x) = 0;
      }
    }
  }

  void searchTomatoBox(cv::Mat& binary_mat, std::vector<cv::Rect>& tomato_boxs) {
    std::vector<std::vector<cv::Point> > contours;
    cv::findContours(binary_mat, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
    findMatchTomato(contours, tomato_boxs);
  }

  void findMatchTomato(std::vector<std::vector<cv::Point> >& contours, std::vector<cv::Rect>& tomato_boxs) {
    cv::Rect bounding_box;
    for (unsigned int i = 0; i < contours.size(); i++) {
      bounding_box = cv::boundingRect(contours[i]);

      float ratio_balance = (float)bounding_box.width / (float)bounding_box.height;
	if (ratio_balance > 1.0f) ratio_balance = 1.0f / ratio_balance;

      // delete mismach. that is smaller or spier
      if (bounding_box.area() >= 500 && bounding_box.area() < 2*cv::contourArea(contours[i]) && ratio_balance > 0.4f)
	tomato_boxs.push_back(bounding_box);
    }
  }

  void applyKalman(std::vector<cv::Rect>& tomato_boxs) {
    cv::Mat meas(4, 1, CV_32F);
    int i = findIndexPlausible(tomato_boxs);

    meas.at<float>(0) = tomato_boxs[i].x + tomato_boxs[i].width/2;
    meas.at<float>(1) = tomato_boxs[i].y + tomato_boxs[i].height/2;
    meas.at<float>(2) = tomato_boxs[i].width;
    meas.at<float>(3) = tomato_boxs[i].height;
    kf.correct(meas);
  }

  int findIndexPlausible(std::vector<cv::Rect>& tomato_boxs) {
    int index = 0;
    unsigned int worst_non_likelihood = 0xffff;
    unsigned int non_likelihood;
    for (int i = 0; i < tomato_boxs.size(); i++) {
      state = kf.predict();
      non_likelihood = 0;
      non_likelihood += abs(tomato_boxs[i].x - state.at<float>(0));
      non_likelihood += abs(tomato_boxs[i].y - state.at<float>(1));
      non_likelihood += abs(tomato_boxs[i].width - state.at<float>(4));
      non_likelihood += abs(tomato_boxs[i].height - state.at<float>(5));
      if (worst_non_likelihood > non_likelihood) {
	worst_non_likelihood = non_likelihood;
	index = i;
      }
    }
    return index;
  }

  void resetKalman() {
//     kf.errorCovPre.at<float>(0) = 1; // px
//     kf.errorCovPre.at<float>(7) = 1; // px
//     kf.errorCovPre.at<float>(14) = 1;
//     kf.errorCovPre.at<float>(21) = 1;
//     kf.errorCovPre.at<float>(28) = 1; // px
//     kf.errorCovPre.at<float>(35) = 1; // px

    for (int i=0; i<6; i++) kf.errorCovPre.at<float>(i*7) = 1;
  }

  bool found_flag;
  long tick_count, last_tick_count; // count while running.
  unsigned int not_founding_count;
  cv::KalmanFilter kf;
  cv::Mat state;
};

class ImageConverter {
private:
  ros::NodeHandle nh;
  ros::Publisher point_pub;
  image_transport::ImageTransport it;
  image_transport::Subscriber rgb_sub;
  image_transport::Subscriber depth_sub;
  cv_bridge::CvImageConstPtr rgb_ptr;
  cv_bridge::CvImageConstPtr depth_ptr;
  SearchTomato searchTomatoObj;
  cv::Point tomato_point;
  bool found_flag;
  geometry_msgs::Point pub_msg;

public:
  ImageConverter()
    : it(nh)
    , searchTomatoObj()
    , tomato_point(-1, -1)
    , found_flag(false)
  {
    // subscrive to input video feed.
    point_pub = nh.advertise<geometry_msgs::Point>("tomato_point", 1);
    rgb_sub = it.subscribe("/camera/rgb/image_raw", 1, &ImageConverter::rgbCb, this);
    depth_sub = it.subscribe("/camera/depth_registered/image_raw", 1, &ImageConverter::depthCb, this);
  }

  ~ImageConverter() {}

  /**
   * search tomato function.
   * get rgb image, and search tomato.
   * will tomato_point have data.
   *
   * @author Yusuke Doi
   */
  void rgbCb(const sensor_msgs::ImageConstPtr& msg) {
    // get rgb image on kinect
    try {
      rgb_ptr = cv_bridge::toCvShare(msg, "bgr8");
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception by rgb: %s", e.what());
    }

    cv::Mat buf = rgb_ptr->image.clone();
    // search tomato
    if (found_flag = searchTomatoObj.searchTomatoPoint(rgb_ptr->image, tomato_point))
      cv::circle(buf, tomato_point, 2, CV_RGB(0, 255, 0), -1);

    cv::imshow(RGB_WINDOW_NAME, buf);
    cv::waitKey(100);
  }

  void depthCb(const sensor_msgs::ImageConstPtr& msg) {
    try {
      depth_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_16UC1);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception by depth: %s", e.what());
    }

    //testShowDepth(depth_ptr->image);
    if (found_flag) {
      printf("depth is %u", depth_ptr->image.at<uint16_t>(tomato_point));
//      pub_msg.x = tomato_point.x;
//      pub_msg.y = tomato_point.y;
//      pub_msg.z = depth_ptr->image.at<uint16_t>(tomato_point);
      pub_msg.x = depth_ptr->image.at<uint16_t>(tomato_point);
      pub_msg.y = tomato_point.x;
      pub_msg.z = -tomato_point.y;
      point_pub.publish(pub_msg);
    }
    cv::waitKey(100);
  }

  void testShowDepth(const cv::Mat& depth) {
    static std::vector<uint16_t> m_gamma(2048);
    static bool firstFlag(false);
    if (!firstFlag) {
      for(unsigned int i = 0; i < 2048; i++) {
	float v = i/2048.0;
	v = std::pow(v, 3) * 6;
	m_gamma[i] = v*6*256;
      }
      firstFlag = true;
    }

    int i;
    cv::Mat buf = cv::Mat::zeros(depth.size(), CV_16UC1);
    cv::Mat output = cv::Mat::zeros(depth.size(), CV_8UC3);
    cv::normalize(depth, buf, 0, 2013, cv::NORM_MINMAX);

    for (i = 0; i < depth.rows * depth.cols; i++) {
      int y = (int)(i / depth.cols);
      int x = (int)(i % depth.cols);
      int pval = m_gamma[buf.at<uint16_t>(i)];
      int lb = pval & 0xff;

      switch(pval >> 8) {
      case 0:
	output.at<cv::Vec3b>(y,x)[2] = 255;
	output.at<cv::Vec3b>(y,x)[1] = 255-lb;
	output.at<cv::Vec3b>(y,x)[0] = 255-lb;
	break;
      case 1:
	output.at<cv::Vec3b>(y,x)[2] = 255;
	output.at<cv::Vec3b>(y,x)[1] = lb;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      case 2:
	output.at<cv::Vec3b>(y,x)[2] = 255-lb;
	output.at<cv::Vec3b>(y,x)[1] = 255;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      case 3:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 255;
	output.at<cv::Vec3b>(y,x)[0] = lb;
	break;
      case 4:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 255-lb;
	output.at<cv::Vec3b>(y,x)[0] = 255;
	break;
      case 5:
	output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 0;
	output.at<cv::Vec3b>(y,x)[0] = 255-lb;
        break;
      default:
        output.at<cv::Vec3b>(y,x)[2] = 0;
	output.at<cv::Vec3b>(y,x)[1] = 0;
	output.at<cv::Vec3b>(y,x)[0] = 0;
	break;
      }
    }
    imshow(DEPTH_WINDOW_NAME, output);
  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "kinect_tomato_search_node");
  ImageConverter ic;
  cv::namedWindow(RGB_WINDOW_NAME);
  cv::namedWindow(DEPTH_WINDOW_NAME);
  ros::spin();
  cv::destroyWindow(RGB_WINDOW_NAME);
  cv::destroyWindow(DEPTH_WINDOW_NAME);
  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2011, Vanadium Labs LLC
 * All Rights Reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Vanadium Labs LLC nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Michael Ferguson, Helen Oleynikova
 */

#include <ros/ros.h>
#include <tf/tf.h>

#include <actionlib/server/simple_action_server.h>
#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <geometry_msgs/PoseArray.h>

namespace turtlebot_arm_block_manipulation
{

class PickAndPlaceServer
{
private:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::PickAndPlaceAction> as_;
  std::string action_name_;

  turtlebot_arm_block_manipulation::PickAndPlaceFeedback     feedback_;
  turtlebot_arm_block_manipulation::PickAndPlaceResult       result_;
  turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr goal_;

  ros::Publisher target_pose_pub_;
  ros::Subscriber pick_and_place_sub_;

  // Move groups to control arm and gripper with MoveIt!
  moveit::planning_interface::MoveGroupInterface arm_;
  moveit::planning_interface::MoveGroupInterface gripper_;

  // Parameters from goal
  std::string arm_link;
  double gripper_open;
  double gripper_closed;
  double z_up;

public:
  PickAndPlaceServer(const std::string name) :
    nh_("~"), as_(name, false), action_name_(name), arm_("pincher_arm"), gripper_("pincher_gripper")
  {
    // Register the goal and feedback callbacks
    as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));

    as_.start();

    target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/target_pose", 1, true);
  }

  void goalCB()
  {
    ROS_INFO("[pick and place] Received goal!");
    goal_ = as_.acceptNewGoal();
    arm_link = goal_->frame;
    gripper_open = goal_->gripper_open;
    gripper_closed = goal_->gripper_closed;
    z_up = goal_->z_up;

    arm_.setPoseReferenceFrame(arm_link);

    // Allow some leeway in position (meters) and orientation (radians)
    arm_.setGoalPositionTolerance(0.001);
    arm_.setGoalOrientationTolerance(0.1);

    // Allow replanning to increase the odds of a solution
    arm_.allowReplanning(true);

    if (goal_->topic.length() < 1)
    {
      pickAndPlace(goal_->pickup_pose, goal_->place_pose);
    }
    else
    {
      pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
    }
  }

  void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr& msg)
  {
    ROS_INFO("[pick and place] Got goal from topic! %s", goal_->topic.c_str());
    pickAndPlace(msg->poses[0], msg->poses[1]);
    pick_and_place_sub_.shutdown();
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
  {
    ROS_INFO("[pick and place] Picking. Also placing.");

    geometry_msgs::Pose target;

    /* open gripper */
    if (setGripper(gripper_open) == false)
      return;

    /* hover over */
    target = start_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = start_pose.position.z - 0.01; //TODO: subtracting 10mm to grab block closer to base
    if (moveArmTo(target) == false)
      return;

    /* close gripper */
    if (setGripper(gripper_closed) == false)
      return;
    ros::Duration(0.8).sleep(); // ensure that gripper properly grasp the cube before lifting the arm

    /* go up */
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* hover over */
    target = end_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = end_pose.position.z;
    if (moveArmTo(target) == false)
      return;

    /* open gripper */
    if (setGripper(gripper_open) == false)
      return;
    ros::Duration(0.6).sleep(); // ensure that gripper properly release the cube before lifting the arm

    /* go up */
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* move out of camera's view */
    target.position.z = z_up;
    if (moveArmTo("pose_navigation_alternate") == false)
      return;

    as_.setSucceeded(result_);
  }

private:
  /**
   * Move arm to a named configuration, normally described in the robot semantic description SRDF file.
   * @param target Named target to achieve
   * @return True of success, false otherwise
   */
  bool moveArmTo(const std::string& target)
  {
    ROS_DEBUG("[pick and place] Move arm to '%s' position", target.c_str());
    if (arm_.setNamedTarget(target) == false)
    {
      ROS_ERROR("[pick and place] Set named target '%s' failed", target.c_str());
      return false;
    }

    moveit::planning_interface::MoveItErrorCode result = arm_.move();
    if (bool(result) == true)
    {
      return true;
    }
    else
    {
      ROS_ERROR("[pick and place] Move to target failed (error %d)", result.val);
      as_.setAborted(result_);
      return false;
    }
  }

  /**
   * Move arm to a target pose. Only position coordinates are taken into account; the
   * orientation is calculated according to the direction and distance to the target.
   * @param target Pose target to achieve
   * @return True of success, false otherwise
   */
  bool moveArmTo(const geometry_msgs::Pose& target)
  {
    int attempts = 0;
    ROS_DEBUG("[pick and place] Move arm to [%.2f, %.2f, %.2f, %.2f]",
             target.position.x, target.position.y, target.position.z, tf::getYaw(target.orientation));
    while (attempts < 5)
    {
      geometry_msgs::PoseStamped modiff_target;
      modiff_target.header.frame_id = arm_link;
      modiff_target.pose = target;

      double x = modiff_target.pose.position.x;
      double y = modiff_target.pose.position.y;
      double z = modiff_target.pose.position.z;
      double d = sqrt(x*x + y*y);
      if (d > 0.3)
      {
        // Maximum reachable distance by the arm is 30 cm
        ROS_ERROR("Target pose out of reach [%f > %f]", d, 0.3);
        as_.setAborted(result_);
        return false;
      }
      // Pitch is 90 (vertical) at 10 cm from the arm base; the farther the target is, the closer to horizontal
      // we point the gripper. Yaw is the direction to the target. We also try some random variations of both to
      // increase the chances of successful planning.
      double rp = M_PI_2 - std::asin((d - 0.1)/0.205); // 0.205 = arm's max reach - vertical pitch distance + ε
      double ry = std::atan2(y, x);

      tf::Quaternion q = tf::createQuaternionFromRPY(0.0,
                                                     attempts*fRand(-0.05, +0.05) + rp,
                                                     attempts*fRand(-0.05, +0.05) + ry);
      tf::quaternionTFToMsg(q, modiff_target.pose.orientation);

      // Slightly increase z proportionally to pitch to avoid hitting the table with the lower gripper corner
      ROS_DEBUG("z increase:  %f  +  %f", modiff_target.pose.position.z, std::abs(std::cos(rp))/50.0);
      modiff_target.pose.position.z += std::abs(std::cos(rp))/50.0;

      ROS_DEBUG("Set pose target [%.2f, %.2f, %.2f] [d: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rp, ry);
      target_pose_pub_.publish(modiff_target);

      if (arm_.setPoseTarget(modiff_target) == false)
      {
        ROS_ERROR("Set pose target [%.2f, %.2f, %.2f, %.2f] failed",
                  modiff_target.pose.position.x, modiff_target.pose.position.y, modiff_target.pose.position.z,
                  tf::getYaw(modiff_target.pose.orientation));
        as_.setAborted(result_);
        return false;
      }

      moveit::planning_interface::MoveItErrorCode result = arm_.move();
      if (bool(result) == true)
      {
        return true;
      }
      else
      {
        ROS_ERROR("[pick and place] Move to target failed (error %d) at attempt %d",
                  result.val, attempts + 1);
      }
      attempts++;
    }

    ROS_ERROR("[pick and place] Move to target failed after %d attempts", attempts);
    as_.setAborted(result_);
    return false;
  }

  /**
   * Set gripper opening.
   * @param opening Physical opening of the gripper, in meters
   * @return True of success, false otherwise
   */
  bool setGripper(float opening)
  {
    ROS_DEBUG("[pick and place] Set gripper opening to %f", opening);
    if (gripper_.setJointValueTarget("gripper_joint", opening) == false)
    {
      ROS_ERROR("[pick and place] Set gripper opening to %f failed", opening);
      return false;
    }

    moveit::planning_interface::MoveItErrorCode result = gripper_.move();
    if (bool(result) == true)
    {
      return true;
    }
    else
    {
      ROS_ERROR("[pick and place] Set gripper opening failed (error %d)", result.val);
      as_.setAborted(result_);
      return false;
    }
  }

  float fRand(float min, float max)
  {
    return ((float(rand()) / float(RAND_MAX)) * (max - min)) + min;
  }
};

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pick_and_place_action_server");

  turtlebot_block_manipulation::PickAndPlaceServer server("pick_and_place");

  ros::spin();

  return 0;
}
<commit_msg>Fix namespace<commit_after>/*
 * Copyright (c) 2011, Vanadium Labs LLC
 * All Rights Reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Vanadium Labs LLC nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Michael Ferguson, Helen Oleynikova
 */

#include <ros/ros.h>
#include <tf/tf.h>

#include <actionlib/server/simple_action_server.h>
#include <turtlebot_arm_block_manipulation/PickAndPlaceAction.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <geometry_msgs/PoseArray.h>

namespace turtlebot_arm_block_manipulation
{

class PickAndPlaceServer
{
private:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::PickAndPlaceAction> as_;
  std::string action_name_;

  turtlebot_arm_block_manipulation::PickAndPlaceFeedback     feedback_;
  turtlebot_arm_block_manipulation::PickAndPlaceResult       result_;
  turtlebot_arm_block_manipulation::PickAndPlaceGoalConstPtr goal_;

  ros::Publisher target_pose_pub_;
  ros::Subscriber pick_and_place_sub_;

  // Move groups to control arm and gripper with MoveIt!
  moveit::planning_interface::MoveGroupInterface arm_;
  moveit::planning_interface::MoveGroupInterface gripper_;

  // Parameters from goal
  std::string arm_link;
  double gripper_open;
  double gripper_closed;
  double z_up;

public:
  PickAndPlaceServer(const std::string name) :
    nh_("~"), as_(name, false), action_name_(name), arm_("pincher_arm"), gripper_("pincher_gripper")
  {
    // Register the goal and feedback callbacks
    as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));

    as_.start();

    target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/target_pose", 1, true);
  }

  void goalCB()
  {
    ROS_INFO("[pick and place] Received goal!");
    goal_ = as_.acceptNewGoal();
    arm_link = goal_->frame;
    gripper_open = goal_->gripper_open;
    gripper_closed = goal_->gripper_closed;
    z_up = goal_->z_up;

    arm_.setPoseReferenceFrame(arm_link);

    // Allow some leeway in position (meters) and orientation (radians)
    arm_.setGoalPositionTolerance(0.001);
    arm_.setGoalOrientationTolerance(0.1);

    // Allow replanning to increase the odds of a solution
    arm_.allowReplanning(true);

    if (goal_->topic.length() < 1)
    {
      pickAndPlace(goal_->pickup_pose, goal_->place_pose);
    }
    else
    {
      pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
    }
  }

  void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr& msg)
  {
    ROS_INFO("[pick and place] Got goal from topic! %s", goal_->topic.c_str());
    pickAndPlace(msg->poses[0], msg->poses[1]);
    pick_and_place_sub_.shutdown();
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
  {
    ROS_INFO("[pick and place] Picking. Also placing.");

    geometry_msgs::Pose target;

    /* open gripper */
    if (setGripper(gripper_open) == false)
      return;

    /* hover over */
    target = start_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = start_pose.position.z - 0.01; //TODO: subtracting 10mm to grab block closer to base
    if (moveArmTo(target) == false)
      return;

    /* close gripper */
    if (setGripper(gripper_closed) == false)
      return;
    ros::Duration(0.8).sleep(); // ensure that gripper properly grasp the cube before lifting the arm

    /* go up */
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* hover over */
    target = end_pose;
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* go down */
    target.position.z = end_pose.position.z;
    if (moveArmTo(target) == false)
      return;

    /* open gripper */
    if (setGripper(gripper_open) == false)
      return;
    ros::Duration(0.6).sleep(); // ensure that gripper properly release the cube before lifting the arm

    /* go up */
    target.position.z = z_up;
    if (moveArmTo(target) == false)
      return;

    /* move out of camera's view */
    target.position.z = z_up;
    if (moveArmTo("pose_navigation_alternate") == false)
      return;

    as_.setSucceeded(result_);
  }

private:
  /**
   * Move arm to a named configuration, normally described in the robot semantic description SRDF file.
   * @param target Named target to achieve
   * @return True of success, false otherwise
   */
  bool moveArmTo(const std::string& target)
  {
    ROS_DEBUG("[pick and place] Move arm to '%s' position", target.c_str());
    if (arm_.setNamedTarget(target) == false)
    {
      ROS_ERROR("[pick and place] Set named target '%s' failed", target.c_str());
      return false;
    }

    moveit::planning_interface::MoveItErrorCode result = arm_.move();
    if (bool(result) == true)
    {
      return true;
    }
    else
    {
      ROS_ERROR("[pick and place] Move to target failed (error %d)", result.val);
      as_.setAborted(result_);
      return false;
    }
  }

  /**
   * Move arm to a target pose. Only position coordinates are taken into account; the
   * orientation is calculated according to the direction and distance to the target.
   * @param target Pose target to achieve
   * @return True of success, false otherwise
   */
  bool moveArmTo(const geometry_msgs::Pose& target)
  {
    int attempts = 0;
    ROS_DEBUG("[pick and place] Move arm to [%.2f, %.2f, %.2f, %.2f]",
             target.position.x, target.position.y, target.position.z, tf::getYaw(target.orientation));
    while (attempts < 5)
    {
      geometry_msgs::PoseStamped modiff_target;
      modiff_target.header.frame_id = arm_link;
      modiff_target.pose = target;

      double x = modiff_target.pose.position.x;
      double y = modiff_target.pose.position.y;
      double z = modiff_target.pose.position.z;
      double d = sqrt(x*x + y*y);
      if (d > 0.3)
      {
        // Maximum reachable distance by the arm is 30 cm
        ROS_ERROR("Target pose out of reach [%f > %f]", d, 0.3);
        as_.setAborted(result_);
        return false;
      }
      // Pitch is 90 (vertical) at 10 cm from the arm base; the farther the target is, the closer to horizontal
      // we point the gripper. Yaw is the direction to the target. We also try some random variations of both to
      // increase the chances of successful planning.
      double rp = M_PI_2 - std::asin((d - 0.1)/0.205); // 0.205 = arm's max reach - vertical pitch distance + ε
      double ry = std::atan2(y, x);

      tf::Quaternion q = tf::createQuaternionFromRPY(0.0,
                                                     attempts*fRand(-0.05, +0.05) + rp,
                                                     attempts*fRand(-0.05, +0.05) + ry);
      tf::quaternionTFToMsg(q, modiff_target.pose.orientation);

      // Slightly increase z proportionally to pitch to avoid hitting the table with the lower gripper corner
      ROS_DEBUG("z increase:  %f  +  %f", modiff_target.pose.position.z, std::abs(std::cos(rp))/50.0);
      modiff_target.pose.position.z += std::abs(std::cos(rp))/50.0;

      ROS_DEBUG("Set pose target [%.2f, %.2f, %.2f] [d: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rp, ry);
      target_pose_pub_.publish(modiff_target);

      if (arm_.setPoseTarget(modiff_target) == false)
      {
        ROS_ERROR("Set pose target [%.2f, %.2f, %.2f, %.2f] failed",
                  modiff_target.pose.position.x, modiff_target.pose.position.y, modiff_target.pose.position.z,
                  tf::getYaw(modiff_target.pose.orientation));
        as_.setAborted(result_);
        return false;
      }

      moveit::planning_interface::MoveItErrorCode result = arm_.move();
      if (bool(result) == true)
      {
        return true;
      }
      else
      {
        ROS_ERROR("[pick and place] Move to target failed (error %d) at attempt %d",
                  result.val, attempts + 1);
      }
      attempts++;
    }

    ROS_ERROR("[pick and place] Move to target failed after %d attempts", attempts);
    as_.setAborted(result_);
    return false;
  }

  /**
   * Set gripper opening.
   * @param opening Physical opening of the gripper, in meters
   * @return True of success, false otherwise
   */
  bool setGripper(float opening)
  {
    ROS_DEBUG("[pick and place] Set gripper opening to %f", opening);
    if (gripper_.setJointValueTarget("gripper_joint", opening) == false)
    {
      ROS_ERROR("[pick and place] Set gripper opening to %f failed", opening);
      return false;
    }

    moveit::planning_interface::MoveItErrorCode result = gripper_.move();
    if (bool(result) == true)
    {
      return true;
    }
    else
    {
      ROS_ERROR("[pick and place] Set gripper opening failed (error %d)", result.val);
      as_.setAborted(result_);
      return false;
    }
  }

  float fRand(float min, float max)
  {
    return ((float(rand()) / float(RAND_MAX)) * (max - min)) + min;
  }
};

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pick_and_place_action_server");

  turtlebot_arm_block_manipulation::PickAndPlaceServer server("pick_and_place");

  ros::spin();

  return 0;
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

  Some parts of this code are derived from ITK. See ITKCopyright.txt
  for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

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

// Software Guide : BeginLatex
//
// The following example illustrates how to segment very large images
// using the \doxygen{otb}{StreamingImageToOGRLayerSegmentationFilter}. This filter is
// templated over the segmentation filter that will be used to segment each tile
// of the input image. In this example we will use the \doxygen{otb}{MeanShiftVectorImageFilter}.
// The labeled output image of each tile is then vectorized (using a filter based on GDALPolygonize)
// and stored into a \doxygen{otb}{ogr}{Layer} within the \doxygen{otb}{ogr}{DataSource}
// set as input. Finally a fusion filter, \doxygen{otb}{OGRDataSourceStreamStitchingFilter}, is used to merge polygons
// at tile border.
//
// Let's take a look at the code.
// First we include all the needed headers
// Software Guide : EndLatex
#include <iostream>

// Software Guide : BeginCodeSnippet
#include "otbStreamingImageToOGRLayerSegmentationFilter.h"
#include "otbMeanShiftVectorImageFilter.h"
#include "otbOGRDataSourceWrapper.h"
#include "otbOGRDataSourceStreamStitchingFilter.h"
// Software Guide : EndCodeSnippet

#include "otbVectorImage.h"
#include "otbImageFileReader.h"

int main(int argc, char *argv[])
{
  if (argc != 13)
    {
      std::cerr << "Usage: " << argv[0];
      std::cerr << " inputImage maskImage outputVec layerName TileDimension"
               << "spatialRadius rangeRadius minObjectSize filterSmallObj minSize"
               << "SimplifyFlag Tolerance" << std::endl;
      return EXIT_FAILURE;
    }

  const char * imageName                    = argv[1];
  const char * maskName                     = argv[2];
  const char * dataSourceName               = argv[3];
  const char * layerName                    = argv[4];
  const unsigned int tileSize               = atoi(argv[5]);
  const unsigned int spatialRadius          = atoi(argv[6]);
  const double rangeRadius                  = atof(argv[7]);
  const unsigned int minimumObjectSize      = atoi(argv[8]);
  const bool filterSmallObj                 = atoi(argv[9]);
  const unsigned int minSize                = atoi(argv[10]);
  const bool simplify                       = atoi(argv[11]);
  const double tolerance                    = atof(argv[12]);
  const std::string fieldName("DN");

  // Software Guide : BeginLatex
  //
  // We now declare the image and pixel types used as input of the MeanShiftFilter.
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef float InputPixelType;
  typedef unsigned int LabelPixelType;

  const unsigned int Dimension = 2;
  typedef otb::VectorImage<InputPixelType,  Dimension>          ImageType;
  typedef otb::Image<LabelPixelType,  Dimension>          LabelImageType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Then the mean shift segmentation filter is declared using the Image type declared previsouly.
  // The StreamingVectorizedSegmentation is templated over the mean shift filter, the input image type and the output vector data type
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  //typedef otb::MeanShiftSmoothingImageFilter<ImageType, ImageType> MeanShiftImageFilterType;
  typedef otb::MeanShiftVectorImageFilter <ImageType, ImageType, LabelImageType>  SegmentationFilterType;
  typedef otb::StreamingImageToOGRLayerSegmentationFilter<ImageType, SegmentationFilterType> StreamingVectorizedSegmentationType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Finaly we define a Reader on the input image and a mask reader.
  // All pixels in the mask with a value of 0 will not be considered suitable for vectorization.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  typedef otb::ImageFileReader<ImageType>           ReaderType;
  typedef otb::ImageFileReader<LabelImageType>      MaskReaderType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now we have declared all type needed for the pipeline, we instantiate the different filters,
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  ReaderType::Pointer        reader = ReaderType::New();
  MaskReaderType::Pointer    maskReader = MaskReaderType::New();
  StreamingVectorizedSegmentationType::Pointer filter = StreamingVectorizedSegmentationType::New();
  // Software Guide : EndCodeSnippet

  reader->SetFileName(imageName);
  reader->UpdateOutputInformation();

  maskReader->SetFileName(maskName);
  maskReader->UpdateOutputInformation();

  // Software Guide : BeginLatex
  //
  // The instanciation of the DataSource is slightly different as usual.
  // In fact the \code{New()} method on a \doxygen{otb}{ogr}{DataSource} can be called with or without parameters.
  // Without parameters, the \code{New()} method instanciate a "Memory" DataSource, which means all the data are stored in memory.
  // This is not useful in case of large scale segmentation as it will result in millions of polygons kept in memory ...
  // However the \code{New()} method can also take a filename (\code{std::String}) parameter. Then either the file already exists
  // and the corresponding ogr driver is used to open the file, or it doesn't exists and then it is created.
  // Here we used a non existing filename to create a new file in writing mode.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  otb::ogr::DataSource::Pointer ogrDS = otb::ogr::DataSource::New(dataSourceName, otb::ogr::DataSource::Modes::write);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Next, we will create the layer inside the DataSource that will
  // hold the polygons from the vectorized segmentation. First, we
  // need to retrieve the spatial reference from the input image.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  OGRSpatialReference oSRS(reader->GetOutput()->GetProjectionRef().c_str());
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now, we can create the layer.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  otb::ogr::Layer ogrLayer = ogrDS->CreateLayer(layerName,&oSRS,wkbMultiPolygon,NULL);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Last, we need to create a field inside the layer that will hold
  // the identifier of the polygons.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  OGRFieldDefn ogrField(fieldName.c_str(),OFTInteger);
  ogrLayer.CreateField(ogrField,true);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now we set the parameters to the segmentation filter.The \doxygen{otb}{MeanShiftVectorImageFilter}
  // required three parameters, the spatial radius, the range radius and the minimum object size.
  // We use the \code{GetSegmentationFilter()} method on the \doxygen{otb}{StreamingVectorizedSegmentation}
  // to get a pointer to the segmentation filter.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->GetSegmentationFilter()->SetSpatialRadius(spatialRadius);
  filter->GetSegmentationFilter()->SetRangeRadius(rangeRadius);
  filter->GetSegmentationFilter()->SetMinimumRegionSize(minimumObjectSize);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Then we set parameters to the \doxygen{otb}{StreamingVectorizedSegmentation} filter.
  // These parameters are :
  // \begin{itemize}
  //  \item tile size : use \code{SetTileDimensionTiledStreaming()} for square tile or \code{SetNumberOfLinesStrippedStreaming()}
  // for Strip.
  // \item field name : name of the field that will contained the label values. (default is "DN").
  // \item start label : first label. Each polygons have a unique label (incremented by one).
  // \item option to filter small polygons (default to false).
  // \item minimum object size : in case filter small polygons option is True
  // \item simplify option : simplification of polygon vertex (default to false).This can reduced very efficiently the size
  // of the output file with no real impact on the results.
  // \item simplification tolerance
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->GetStreamer()->SetTileDimensionTiledStreaming(tileSize);
  filter->SetFieldName(fieldName);
  filter->SetStartLabel(1);
  filter->SetFilterSmallObject(filterSmallObj);
  filter->SetMinimumObjectSize(minSize);
  filter->SetSimplify(simplify);
  filter->SetSimplificationTolerance(tolerance);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Finally we connect the pipeline
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->SetInput(reader->GetOutput());
  filter->SetInputMask(maskReader->GetOutput());
  filter->SetOGRLayer(ogrLayer);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // And call the \code{Initialize()} method (needed to create the output layer in the datasource)
  // before calling the \code{Update()} method.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->Initialize();
  filter->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // The segmentation is done, but as it works tile by tile, we need to fusion polygons at tile border.
  // We use the \doxygen{otb}{OGRDataSourceStreamStitchingFilter}. This filter uses a simple fusion strategy.
  // Polygons that have the largest intersection over a tile are fusioned. Each polygon can be fusioned
  // only once per tile border (row and column).
  // Let's look at the code for fusioning.
  // As usual we declared and instanciate the \doxygen{otb}{OGRDataSourceStreamStitchingFilter}.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  typedef otb::OGRDataSourceStreamStitchingFilter<ImageType>   FusionFilterType;
  FusionFilterType::Pointer fusionFilter = FusionFilterType::New();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // Next we set the input image and the input \doxygen{otb}{ogr}{DataSource}.
  // The image is internally used in the filter to compute coordinates of streaming tiles.
  // The DataSource is the one containing the segmentation results.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  fusionFilter->SetInput(reader->GetOutput());
  fusionFilter->SetOGRDataSource(ogrDS);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // We set the name of the layer containing segmentation results which is the same that we used
  // for the \doxygen{otb}{StreamingVectorizedSegmentation} filter. We also set the size of the
  // tile used, which may be different from the one we set in the \doxygen{otb}{StreamingVectorizedSegmentation} filter
  // but can be retrieved using the \code{GetStreamSize()} method.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  fusionFilter->SetStreamSize(filter->GetStreamSize());
  fusionFilter->SetLayerName(layerName);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // Finally we call the \code{GenerateData()} method to launch the processing.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  fusionFilter->GenerateData();
  // Software Guide : EndCodeSnippet

  return EXIT_SUCCESS;
}
<commit_msg>DOC: Adapting software guide example following API change<commit_after>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

  Some parts of this code are derived from ITK. See ITKCopyright.txt
  for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

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

// Software Guide : BeginLatex
//
// The following example illustrates how to segment very large images
// using the \doxygen{otb}{StreamingImageToOGRLayerSegmentationFilter}. This filter is
// templated over the segmentation filter that will be used to segment each tile
// of the input image. In this example we will use the \doxygen{otb}{MeanShiftVectorImageFilter}.
// The labeled output image of each tile is then vectorized (using a filter based on GDALPolygonize)
// and stored into a \doxygen{otb}{ogr}{Layer} within the \doxygen{otb}{ogr}{DataSource}
// set as input. Finally a fusion filter, \doxygen{otb}{OGRLayerStreamStitchingFilter}, is used to merge polygons
// at tile border.
//
// Let's take a look at the code.
// First we include all the needed headers
// Software Guide : EndLatex
#include <iostream>

// Software Guide : BeginCodeSnippet
#include "otbStreamingImageToOGRLayerSegmentationFilter.h"
#include "otbMeanShiftVectorImageFilter.h"
#include "otbOGRDataSourceWrapper.h"
#include "otbOGRLayerStreamStitchingFilter.h"
// Software Guide : EndCodeSnippet

#include "otbVectorImage.h"
#include "otbImageFileReader.h"

int main(int argc, char *argv[])
{
  if (argc != 13)
    {
      std::cerr << "Usage: " << argv[0];
      std::cerr << " inputImage maskImage outputVec layerName TileDimension"
               << "spatialRadius rangeRadius minObjectSize filterSmallObj minSize"
               << "SimplifyFlag Tolerance" << std::endl;
      return EXIT_FAILURE;
    }

  const char * imageName                    = argv[1];
  const char * maskName                     = argv[2];
  const char * dataSourceName               = argv[3];
  const char * layerName                    = argv[4];
  const unsigned int tileSize               = atoi(argv[5]);
  const unsigned int spatialRadius          = atoi(argv[6]);
  const double rangeRadius                  = atof(argv[7]);
  const unsigned int minimumObjectSize      = atoi(argv[8]);
  const bool filterSmallObj                 = atoi(argv[9]);
  const unsigned int minSize                = atoi(argv[10]);
  const bool simplify                       = atoi(argv[11]);
  const double tolerance                    = atof(argv[12]);
  const std::string fieldName("DN");

  // Software Guide : BeginLatex
  //
  // We now declare the image and pixel types used as input of the MeanShiftFilter.
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef float InputPixelType;
  typedef unsigned int LabelPixelType;

  const unsigned int Dimension = 2;
  typedef otb::VectorImage<InputPixelType,  Dimension>          ImageType;
  typedef otb::Image<LabelPixelType,  Dimension>          LabelImageType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Then the mean shift segmentation filter is declared using the Image type declared previsouly.
  // The StreamingVectorizedSegmentation is templated over the mean shift filter, the input image type and the output vector data type
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  //typedef otb::MeanShiftSmoothingImageFilter<ImageType, ImageType> MeanShiftImageFilterType;
  typedef otb::MeanShiftVectorImageFilter <ImageType, ImageType, LabelImageType>  SegmentationFilterType;
  typedef otb::StreamingImageToOGRLayerSegmentationFilter<ImageType, SegmentationFilterType> StreamingVectorizedSegmentationType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Finaly we define a Reader on the input image and a mask reader.
  // All pixels in the mask with a value of 0 will not be considered suitable for vectorization.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  typedef otb::ImageFileReader<ImageType>           ReaderType;
  typedef otb::ImageFileReader<LabelImageType>      MaskReaderType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now we have declared all type needed for the pipeline, we instantiate the different filters,
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  ReaderType::Pointer        reader = ReaderType::New();
  MaskReaderType::Pointer    maskReader = MaskReaderType::New();
  StreamingVectorizedSegmentationType::Pointer filter = StreamingVectorizedSegmentationType::New();
  // Software Guide : EndCodeSnippet

  reader->SetFileName(imageName);
  reader->UpdateOutputInformation();

  maskReader->SetFileName(maskName);
  maskReader->UpdateOutputInformation();

  // Software Guide : BeginLatex
  //
  // The instanciation of the DataSource is slightly different as usual.
  // In fact the \code{New()} method on a \doxygen{otb}{ogr}{DataSource} can be called with or without parameters.
  // Without parameters, the \code{New()} method instanciate a "Memory" DataSource, which means all the data are stored in memory.
  // This is not useful in case of large scale segmentation as it will result in millions of polygons kept in memory ...
  // However the \code{New()} method can also take a filename (\code{std::String}) parameter. Then either the file already exists
  // and the corresponding ogr driver is used to open the file, or it doesn't exists and then it is created.
  // Here we used a non existing filename to create a new file in writing mode.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  otb::ogr::DataSource::Pointer ogrDS = otb::ogr::DataSource::New(dataSourceName, otb::ogr::DataSource::Modes::write);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Next, we will create the layer inside the DataSource that will
  // hold the polygons from the vectorized segmentation. First, we
  // need to retrieve the spatial reference from the input image.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  OGRSpatialReference oSRS(reader->GetOutput()->GetProjectionRef().c_str());
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now, we can create the layer.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  otb::ogr::Layer ogrLayer = ogrDS->CreateLayer(layerName,&oSRS,wkbMultiPolygon,NULL);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Last, we need to create a field inside the layer that will hold
  // the identifier of the polygons.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  OGRFieldDefn ogrField(fieldName.c_str(),OFTInteger);
  ogrLayer.CreateField(ogrField,true);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Now we set the parameters to the segmentation filter.The \doxygen{otb}{MeanShiftVectorImageFilter}
  // required three parameters, the spatial radius, the range radius and the minimum object size.
  // We use the \code{GetSegmentationFilter()} method on the \doxygen{otb}{StreamingVectorizedSegmentation}
  // to get a pointer to the segmentation filter.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->GetSegmentationFilter()->SetSpatialRadius(spatialRadius);
  filter->GetSegmentationFilter()->SetRangeRadius(rangeRadius);
  filter->GetSegmentationFilter()->SetMinimumRegionSize(minimumObjectSize);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Then we set parameters to the \doxygen{otb}{StreamingVectorizedSegmentation} filter.
  // These parameters are :
  // \begin{itemize}
  //  \item tile size : use \code{SetTileDimensionTiledStreaming()} for square tile or \code{SetNumberOfLinesStrippedStreaming()}
  // for Strip.
  // \item field name : name of the field that will contained the label values. (default is "DN").
  // \item start label : first label. Each polygons have a unique label (incremented by one).
  // \item option to filter small polygons (default to false).
  // \item minimum object size : in case filter small polygons option is True
  // \item simplify option : simplification of polygon vertex (default to false).This can reduced very efficiently the size
  // of the output file with no real impact on the results.
  // \item simplification tolerance
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->GetStreamer()->SetTileDimensionTiledStreaming(tileSize);
  filter->SetFieldName(fieldName);
  filter->SetStartLabel(1);
  filter->SetFilterSmallObject(filterSmallObj);
  filter->SetMinimumObjectSize(minSize);
  filter->SetSimplify(simplify);
  filter->SetSimplificationTolerance(tolerance);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Finally we connect the pipeline
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->SetInput(reader->GetOutput());
  filter->SetInputMask(maskReader->GetOutput());
  filter->SetOGRLayer(ogrLayer);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // And call the \code{Initialize()} method (needed to create the output layer in the datasource)
  // before calling the \code{Update()} method.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  filter->Initialize();
  filter->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // The segmentation is done, but as it works tile by tile, we need to fusion polygons at tile border.
  // We use the \doxygen{otb}{OGRLayerStreamStitchingFilter}. This filter uses a simple fusion strategy.
  // Polygons that have the largest intersection over a tile are fusioned. Each polygon can be fusioned
  // only once per tile border (row and column).
  // Let's look at the code for fusioning.
  // As usual we declared and instanciate the \doxygen{otb}{OGRLayerStreamStitchingFilter}.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  typedef otb::OGRLayerStreamStitchingFilter<ImageType>   FusionFilterType;
  FusionFilterType::Pointer fusionFilter = FusionFilterType::New();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // Next we set the input image and the input \doxygen{otb}{ogr}{Layer}.
  // The image is internally used in the filter to compute coordinates of streaming tiles.
  // The DataSource is the one containing the segmentation results.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  fusionFilter->SetInput(reader->GetOutput());
  fusionFilter->SetOGRLayer(ogrLayer);
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // We set the size of the tile used,
  // which may be different from the one we set in the
  // \doxygen{otb}{StreamingVectorizedSegmentation} filter but can be
  // retrieved using the \code{GetStreamSize()} method.
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  fusionFilter->SetStreamSize(filter->GetStreamSize());
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  // Finally we call the \code{GenerateData()} method to launch the processing.
  // Software Guide : EndLatex
  // Software Guide : BeginCodeSnippet
  fusionFilter->GenerateData();
  // Software Guide : EndCodeSnippet

  return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>//*****************************************************************************
//
//	Copyright 2015 Microsoft Corporation
//
//	Licensed under the Apache License, Version 2.0 (the "License");
//	you may not use this file except in compliance with the License.
//	You may obtain a copy of the License at
//
//	http ://www.apache.org/licenses/LICENSE-2.0
//
//	Unless required by applicable law or agreed to in writing, software
//	distributed under the License is distributed on an "AS IS" BASIS,
//	WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//	See the License for the specific language governing permissions and
//	limitations under the License.
//
//*****************************************************************************

#include "pch.h"

#include "UncompressedAudioSampleProvider.h"

using namespace FFmpegInterop;

// Minimum duration for uncompressed audio samples (50 ms)
const LONGLONG MINAUDIOSAMPLEDURATION = 500000;

UncompressedAudioSampleProvider::UncompressedAudioSampleProvider(
	FFmpegReader^ reader,
	AVFormatContext* avFormatCtx,
	AVCodecContext* avCodecCtx)
	: UncompressedSampleProvider(reader, avFormatCtx, avCodecCtx)
	, m_pSwrCtx(nullptr)
{
}

HRESULT UncompressedAudioSampleProvider::AllocateResources()
{
	HRESULT hr = S_OK;
	hr = UncompressedSampleProvider::AllocateResources();
	if (SUCCEEDED(hr))
	{
		// Set default channel layout when the value is unknown (0)
		int64 inChannelLayout = m_pAvCodecCtx->channel_layout ? m_pAvCodecCtx->channel_layout : av_get_default_channel_layout(m_pAvCodecCtx->channels);
		int64 outChannelLayout = av_get_default_channel_layout(m_pAvCodecCtx->channels);

		// Set up resampler to convert any PCM format (e.g. AV_SAMPLE_FMT_FLTP) to AV_SAMPLE_FMT_S16 PCM format that is expected by Media Element.
		// Additional logic can be added to avoid resampling PCM data that is already in AV_SAMPLE_FMT_S16_PCM.
		m_pSwrCtx = swr_alloc_set_opts(
			NULL,
			outChannelLayout,
			AV_SAMPLE_FMT_S16,
			m_pAvCodecCtx->sample_rate,
			inChannelLayout,
			m_pAvCodecCtx->sample_fmt,
			m_pAvCodecCtx->sample_rate,
			0,
			NULL);

		if (!m_pSwrCtx)
		{
			hr = E_OUTOFMEMORY;
		}
	}

	if (SUCCEEDED(hr))
	{
		if (swr_init(m_pSwrCtx) < 0)
		{
			hr = E_FAIL;
		}
	}

	return hr;
}

UncompressedAudioSampleProvider::~UncompressedAudioSampleProvider()
{
	if (m_pAvFrame)
	{
		av_frame_free(&m_pAvFrame);
	}

	// Free
	swr_free(&m_pSwrCtx);
}

HRESULT UncompressedAudioSampleProvider::WriteAVPacketToStream(DataWriter^ dataWriter, AVPacket* avPacket)
{
	// Because each packet can contain multiple frames, we have already written the packet to the stream
	// during the decode stage.
	return S_OK;
}

HRESULT UncompressedAudioSampleProvider::ProcessDecodedFrame(DataWriter^ dataWriter)
{
	// Resample uncompressed frame to AV_SAMPLE_FMT_S16 PCM format that is expected by Media Element
	uint8_t *resampledData = nullptr;
	unsigned int aBufferSize = av_samples_alloc(&resampledData, NULL, m_pAvFrame->channels, m_pAvFrame->nb_samples, AV_SAMPLE_FMT_S16, 0);
	int resampledDataSize = swr_convert(m_pSwrCtx, &resampledData, aBufferSize, (const uint8_t **)m_pAvFrame->extended_data, m_pAvFrame->nb_samples);
	auto aBuffer = ref new Platform::Array<uint8_t>(resampledData, min(aBufferSize, (unsigned int)(resampledDataSize * m_pAvFrame->channels * av_get_bytes_per_sample(AV_SAMPLE_FMT_S16))));
	dataWriter->WriteBytes(aBuffer);
	av_freep(&resampledData);
	av_frame_unref(m_pAvFrame);
	av_frame_free(&m_pAvFrame);

	return S_OK;
}

MediaStreamSample^ UncompressedAudioSampleProvider::GetNextSample()
{
	// Similar to GetNextSample in MediaSampleProvider,
	// but we concatenate samples until reaching a minimum duration
	DebugMessage(L"GetNextSample\n");

	HRESULT hr = S_OK;

	MediaStreamSample^ sample;
	DataWriter^ dataWriter = ref new DataWriter();

	LONGLONG finalPts = -1;
	LONGLONG finalDur = 0;

	do
	{
		LONGLONG pts = 0;
		LONGLONG dur = 0;

		hr = GetNextPacket(dataWriter, pts, dur);

		if (finalPts == -1)
		{
			finalPts = pts;
		}
		finalDur += dur;

	} while (SUCCEEDED(hr) && finalDur < MINAUDIOSAMPLEDURATION);

	if (finalDur > 0)
	{
		sample = MediaStreamSample::CreateFromBuffer(dataWriter->DetachBuffer(), { finalPts });
		sample->Duration = { finalDur };
	}

	return sample;
}
<commit_msg>finalPts and finalDur should not be updated on failure<commit_after>//*****************************************************************************
//
//	Copyright 2015 Microsoft Corporation
//
//	Licensed under the Apache License, Version 2.0 (the "License");
//	you may not use this file except in compliance with the License.
//	You may obtain a copy of the License at
//
//	http ://www.apache.org/licenses/LICENSE-2.0
//
//	Unless required by applicable law or agreed to in writing, software
//	distributed under the License is distributed on an "AS IS" BASIS,
//	WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//	See the License for the specific language governing permissions and
//	limitations under the License.
//
//*****************************************************************************

#include "pch.h"

#include "UncompressedAudioSampleProvider.h"

using namespace FFmpegInterop;

// Minimum duration for uncompressed audio samples (50 ms)
const LONGLONG MINAUDIOSAMPLEDURATION = 500000;

UncompressedAudioSampleProvider::UncompressedAudioSampleProvider(
	FFmpegReader^ reader,
	AVFormatContext* avFormatCtx,
	AVCodecContext* avCodecCtx)
	: UncompressedSampleProvider(reader, avFormatCtx, avCodecCtx)
	, m_pSwrCtx(nullptr)
{
}

HRESULT UncompressedAudioSampleProvider::AllocateResources()
{
	HRESULT hr = S_OK;
	hr = UncompressedSampleProvider::AllocateResources();
	if (SUCCEEDED(hr))
	{
		// Set default channel layout when the value is unknown (0)
		int64 inChannelLayout = m_pAvCodecCtx->channel_layout ? m_pAvCodecCtx->channel_layout : av_get_default_channel_layout(m_pAvCodecCtx->channels);
		int64 outChannelLayout = av_get_default_channel_layout(m_pAvCodecCtx->channels);

		// Set up resampler to convert any PCM format (e.g. AV_SAMPLE_FMT_FLTP) to AV_SAMPLE_FMT_S16 PCM format that is expected by Media Element.
		// Additional logic can be added to avoid resampling PCM data that is already in AV_SAMPLE_FMT_S16_PCM.
		m_pSwrCtx = swr_alloc_set_opts(
			NULL,
			outChannelLayout,
			AV_SAMPLE_FMT_S16,
			m_pAvCodecCtx->sample_rate,
			inChannelLayout,
			m_pAvCodecCtx->sample_fmt,
			m_pAvCodecCtx->sample_rate,
			0,
			NULL);

		if (!m_pSwrCtx)
		{
			hr = E_OUTOFMEMORY;
		}
	}

	if (SUCCEEDED(hr))
	{
		if (swr_init(m_pSwrCtx) < 0)
		{
			hr = E_FAIL;
		}
	}

	return hr;
}

UncompressedAudioSampleProvider::~UncompressedAudioSampleProvider()
{
	if (m_pAvFrame)
	{
		av_frame_free(&m_pAvFrame);
	}

	// Free
	swr_free(&m_pSwrCtx);
}

HRESULT UncompressedAudioSampleProvider::WriteAVPacketToStream(DataWriter^ dataWriter, AVPacket* avPacket)
{
	// Because each packet can contain multiple frames, we have already written the packet to the stream
	// during the decode stage.
	return S_OK;
}

HRESULT UncompressedAudioSampleProvider::ProcessDecodedFrame(DataWriter^ dataWriter)
{
	// Resample uncompressed frame to AV_SAMPLE_FMT_S16 PCM format that is expected by Media Element
	uint8_t *resampledData = nullptr;
	unsigned int aBufferSize = av_samples_alloc(&resampledData, NULL, m_pAvFrame->channels, m_pAvFrame->nb_samples, AV_SAMPLE_FMT_S16, 0);
	int resampledDataSize = swr_convert(m_pSwrCtx, &resampledData, aBufferSize, (const uint8_t **)m_pAvFrame->extended_data, m_pAvFrame->nb_samples);
	auto aBuffer = ref new Platform::Array<uint8_t>(resampledData, min(aBufferSize, (unsigned int)(resampledDataSize * m_pAvFrame->channels * av_get_bytes_per_sample(AV_SAMPLE_FMT_S16))));
	dataWriter->WriteBytes(aBuffer);
	av_freep(&resampledData);
	av_frame_unref(m_pAvFrame);
	av_frame_free(&m_pAvFrame);

	return S_OK;
}

MediaStreamSample^ UncompressedAudioSampleProvider::GetNextSample()
{
	// Similar to GetNextSample in MediaSampleProvider,
	// but we concatenate samples until reaching a minimum duration
	DebugMessage(L"GetNextSample\n");

	HRESULT hr = S_OK;

	MediaStreamSample^ sample;
	DataWriter^ dataWriter = ref new DataWriter();

	LONGLONG finalPts = -1;
	LONGLONG finalDur = 0;

	do
	{
		LONGLONG pts = 0;
		LONGLONG dur = 0;

		hr = GetNextPacket(dataWriter, pts, dur);

		if (SUCCEEDED(hr))
		{
			if (finalPts == -1)
			{
				finalPts = pts;
			}
			finalDur += dur;
		}

	} while (SUCCEEDED(hr) && finalDur < MINAUDIOSAMPLEDURATION);

	if (finalDur > 0)
	{
		sample = MediaStreamSample::CreateFromBuffer(dataWriter->DetachBuffer(), { finalPts });
		sample->Duration = { finalDur };
	}

	return sample;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <fstream>
#include <string>

// Loriano: let's try Armadillo quick code
#include <armadillo>

#define ENTDIM 8
#define COORDIM (ENTDIM-2)
#define PARAMDIM 5

namespace
{
  int numofline (const char * fname)
  {
    int number_of_lines = 0;
    std::string line;
    std::ifstream myfile(fname);

    while (std::getline(myfile, line))
      ++number_of_lines;

    myfile.close();

    return number_of_lines;
  }
}

int main (int argc, char ** argv)
{
  if (argc != 2)
  {
    std::cerr << "usage: " << argv[0] << " coordinatesfile " << std::endl;
    return 1;
  }

  int num_of_line = numofline(argv[1]);
  std::cout << "file has " << num_of_line << " line " << std::endl;
  int num_of_ent = (num_of_line-1)/ENTDIM;
  std::cout << "  " << num_of_ent << " entries " << std::endl;

  // non perfomante ma easy to go
  double ** param_mtx = new double *[num_of_ent];
  double ** coord_mtx = new double *[num_of_ent];
  for (int i = 0; i < num_of_ent; ++i)
  {
    coord_mtx[i] = new double[3*COORDIM];
    param_mtx[i] = new double[PARAMDIM];
  }

  // leggere file coordinate tracce simulate plus parametri
  std::string line;
  std::ifstream mytfp;
  mytfp.open (argv[1], std::ios::in);

  std::getline (mytfp, line);
  //std::cout << line << std::endl;

  for (int i = 0; i < num_of_ent; ++i)
  {
    int fake1, fake2;
    mytfp >> fake1 >> fake2 ;
#ifdef DEBUG
    std::cout << fake1 << " " << fake2 << std::endl;
#endif
    for (int j = 0; j < COORDIM; ++j)
    {
      int a, b, c;
      mytfp >> coord_mtx[i][j*3] >>
               coord_mtx[i][j*3+1] >>
               coord_mtx[i][j*3+2] >>
               a >> b >> c;
    }
    mytfp >> param_mtx[i][0] >>
             param_mtx[i][1] >>
             param_mtx[i][2] >>
             param_mtx[i][3] >>
             param_mtx[i][4];
  }

  mytfp.close();

#ifdef DEBUG
  for (int i = 0; i < num_of_ent; ++i)
  {
    for (int j = 0; j < COORDIM; ++j)
    {
      std::cout << coord_mtx[i][j*3] << " " <<
                   coord_mtx[i][j*3+1] << " " <<
                   coord_mtx[i][j*3+2] << std::endl;
    }
    std::cout << param_mtx[i][0] << " " <<
                 param_mtx[i][1] << " " <<
                 param_mtx[i][2] << " " <<
                 param_mtx[i][3] << " " <<
                 param_mtx[i][4] << std::endl;
  }
#endif

  double sum = 1.0e0;
  arma::mat coordm = arma::zeros<arma::mat>(3*COORDIM);
  arma::mat hca = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);

  for (int l=0; l<num_of_ent; ++l)
  {
    sum += 1.0e0;
    for (int i=0; i<(3*COORDIM); ++i)
      coordm(i) += (coord_mtx[l][i]-coordm(i))/sum;

    for (int i=0; i<(3*COORDIM); ++i)
    {
      for (int j=0; j<(3*COORDIM); ++j)
      {
        hca(i,j) += ((coord_mtx[l][i] - coordm(i))*
                     (coord_mtx[l][j] - coordm(j))-
                     (sum-1.0e0)*hca(i,j)/sum)/(sum-1.0e0);
      }
    }
  }

  /* correlation matrix
  double cstdev[3*COORDIM];
  for (int i=0; i<(3*COORDIM); ++i)
  {
    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(coord_mtx[l][i]);

#ifdef DEBUG
    std::cout << "mean  = " << stats.mean() << std::endl;
    std::cout << "        "  << coordm[i] << std::endl;
    std::cout << "stdev = " << stats.stddev()  << std::endl;
#endif

    cstdev[i] = stats.stddev();
  }

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<(3*COORDIM); ++j)
      hca(i,j) = hca(i,j) / (cstdev[i]*cstdev[j]);
  */

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=i+1; j<(3*COORDIM); ++j)
      if (hca(i,j) != hca(j,i))
        std::cout << i << " " << j << " " <<
          hca(i,j) << " ERROR" << std::endl;;

  arma::vec eigval = arma::zeros<arma::mat>(3*COORDIM);
  arma::mat eigvec = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);

  arma::eig_sym(eigval, eigvec, hca);

  double totval = 0.0e0;
  for (int i=0; i<(3*COORDIM); ++i)
    totval += eigval(i);

  int j = 1;
  double totvar = 0.0e0;
  for (int i=(3*COORDIM-1); i>=0; --i)
  {
    if (j <= PARAMDIM)
      totvar += 100.0e0*(eigval(i)/totval);
    ++j;

#ifdef DEBUG
    std::cout << i+1 << " ==> " << 100.0e0*(eigval(i)/totval)
      << " ==> " << eigval(i) <<  std::endl;
#endif

  }
  std::cout << "PARAMDIM eigenvalues: " << totvar << std::endl;

  arma::mat hcai = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);
  hcai = hca.i();

#ifdef DEBUG
  std::cout << hca * hcai ;
#endif

  // and so on ...
  arma::mat paramm = arma::zeros<arma::mat>(PARAMDIM);
  arma::mat hcap = arma::zeros<arma::mat>(3*COORDIM,PARAMDIM);

  coordm.fill(0.0e0);
  sum = 1.0e0;

  for (int l=0; l<num_of_ent; ++l)
  {
    sum += 1.0e0;
    for (int i=0; i<(3*COORDIM); ++i)
      coordm(i) += (coord_mtx[l][i]-coordm(i))/sum;

    for (int i=0; i<PARAMDIM; ++i)
      paramm(i) += (param_mtx[l][i]-paramm(i))/sum;

    for (int i=0; i<(3*COORDIM); ++i)
    {
      for (int j=0; j<PARAMDIM; ++j)
      {
        hcap(i,j) += ((coord_mtx[l][i] - coordm(i))*
                      (param_mtx[l][j] - paramm(j))-
                      (sum-1.0e0)*hcap(i,j)/sum)/(sum-1.0e0);
      }
    }
  }

  /* correlation matrix
  double pstdev[PARAMDIM];
  for (int i=0; i<PARAMDIM; ++i)
  {
    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(param_mtx[l][i]);

#ifdef DEBUG
    std::cout << "mean   = " << stats.mean() << std::endl;
    std::cout << "         "  << paramm(i) << std::endl;
    std::cout << "stdev  = " << stats.stddev()  << std::endl;
#endif

    pstdev[i] = stats.stddev();
  }

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<PARAMDIM; ++j)
      hcap(i,j) = hcap(i,j) / (cstdev[i]*pstdev[j]);
  */

  arma::mat cmtx = arma::zeros<arma::mat>(PARAMDIM,3*COORDIM);

  for (int i=0; i<PARAMDIM; ++i)
    for (int l=0; l<(3*COORDIM); ++l)
      for (int m=0; m<(3*COORDIM); ++m)
        cmtx(i,l) += hcai(l,m) * hcap (m,i);

#ifdef DEBUG
  std::cout << "C matrix: " << std::endl;
  std::cout << cmtx;
#endif

  arma::mat q = arma::zeros<arma::mat>(PARAMDIM);

  for (int i=0; i<PARAMDIM; ++i)
  {
    q(i) = paramm(i);
    for (int l=0; l<(3*COORDIM); ++l)
      q(i) -= cmtx(i,l)*coordm[l];
  }

#ifdef DEBUG
  std::cout << "Q vector: " << std::endl;
  for (int i=0; i<PARAMDIM; ++i)
    std::cout << q(i) << std::endl;
#endif

  arma::mat k = arma::zeros<arma::mat>((3*COORDIM)-PARAMDIM);

  /* chi**2 */
  double chi2 = 0.0e0;
  for (int i=0; i<(3*COORDIM)-PARAMDIM; ++i)
  {
    double v = 0.0e0;

    for (int ki=0; ki<(3*COORDIM); ++ki)
      k(i) = eigvec(i,ki)*coordm[ki];

    for (int j=0; j<(3*COORDIM); ++j)
      v += (eigvec(i,j)/sqrt(eigval(i))) + k(i);

    chi2 += (v*v);
  }

  std::cout << "Chi2: " << chi2 << std::endl;

  //test back
  arma::running_stat<double> pc[PARAMDIM];
  for (int l=0; l<num_of_ent; ++l)
  {
    for (int i=0; i<PARAMDIM; ++i)
    {
      double p = q(i);
      for (int k=0; k<(3*COORDIM); ++k)
        p += cmtx(i,k)*coord_mtx[l][k];

      pc[i](fabs(p - param_mtx[l][i])/(fabs(p + param_mtx[l][i])/2.0));
    }
  }

  for (int i=0; i<PARAMDIM; ++i)
  {
    std::cout << pc[i].mean() << " " << pc[i].stddev() << std::endl;

    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(param_mtx[l][i]);

    std::cout << "  mean   = " << stats.mean() << std::endl;
    std::cout << "  stdev  = " << stats.stddev()  << std::endl;
    std::cout << "  min    = " << stats.min() << std::endl;
    std::cout << "  max    = " << stats.max() << std::endl;
  }

  for (int i = 0; i < num_of_ent; ++i)
  {
    delete(coord_mtx[i]);
    delete(param_mtx[i]);
  }
  delete(coord_mtx);
  delete(param_mtx);

  return 0;
}
<commit_msg>chi2 first test implementation<commit_after>#include <iostream>
#include <fstream>
#include <string>

// Loriano: let's try Armadillo quick code
#include <armadillo>

#define ENTDIM 8
#define COORDIM (ENTDIM-2)
#define PARAMDIM 5

namespace
{
  int numofline (const char * fname)
  {
    int number_of_lines = 0;
    std::string line;
    std::ifstream myfile(fname);

    while (std::getline(myfile, line))
      ++number_of_lines;

    myfile.close();

    return number_of_lines;
  }
}

int main (int argc, char ** argv)
{
  if (argc != 2)
  {
    std::cerr << "usage: " << argv[0] << " coordinatesfile " << std::endl;
    return 1;
  }

  int num_of_line = numofline(argv[1]);
  std::cout << "file has " << num_of_line << " line " << std::endl;
  int num_of_ent = (num_of_line-1)/ENTDIM;
  std::cout << "  " << num_of_ent << " entries " << std::endl;

  // non perfomante ma easy to go
  double ** param_mtx = new double *[num_of_ent];
  double ** coord_mtx = new double *[num_of_ent];
  for (int i = 0; i < num_of_ent; ++i)
  {
    coord_mtx[i] = new double[3*COORDIM];
    param_mtx[i] = new double[PARAMDIM];
  }

  // leggere file coordinate tracce simulate plus parametri
  std::string line;
  std::ifstream mytfp;
  mytfp.open (argv[1], std::ios::in);

  std::getline (mytfp, line);
  //std::cout << line << std::endl;

  for (int i = 0; i < num_of_ent; ++i)
  {
    int fake1, fake2;
    mytfp >> fake1 >> fake2 ;
#ifdef DEBUG
    std::cout << fake1 << " " << fake2 << std::endl;
#endif
    for (int j = 0; j < COORDIM; ++j)
    {
      int a, b, c;
      mytfp >> coord_mtx[i][j*3] >>
               coord_mtx[i][j*3+1] >>
               coord_mtx[i][j*3+2] >>
               a >> b >> c;
    }
    mytfp >> param_mtx[i][0] >>
             param_mtx[i][1] >>
             param_mtx[i][2] >>
             param_mtx[i][3] >>
             param_mtx[i][4];
  }

  mytfp.close();

#ifdef DEBUG
  for (int i = 0; i < num_of_ent; ++i)
  {
    for (int j = 0; j < COORDIM; ++j)
    {
      std::cout << coord_mtx[i][j*3] << " " <<
                   coord_mtx[i][j*3+1] << " " <<
                   coord_mtx[i][j*3+2] << std::endl;
    }
    std::cout << param_mtx[i][0] << " " <<
                 param_mtx[i][1] << " " <<
                 param_mtx[i][2] << " " <<
                 param_mtx[i][3] << " " <<
                 param_mtx[i][4] << std::endl;
  }
#endif

  double sum = 1.0e0;
  arma::mat coordm = arma::zeros<arma::mat>(3*COORDIM);
  arma::mat hca = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);

  for (int l=0; l<num_of_ent; ++l)
  {
    sum += 1.0e0;
    for (int i=0; i<(3*COORDIM); ++i)
      coordm(i) += (coord_mtx[l][i]-coordm(i))/sum;

    for (int i=0; i<(3*COORDIM); ++i)
    {
      for (int j=0; j<(3*COORDIM); ++j)
      {
        hca(i,j) += ((coord_mtx[l][i] - coordm(i))*
                     (coord_mtx[l][j] - coordm(j))-
                     (sum-1.0e0)*hca(i,j)/sum)/(sum-1.0e0);
      }
    }
  }

  /* correlation matrix
  double cstdev[3*COORDIM];
  for (int i=0; i<(3*COORDIM); ++i)
  {
    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(coord_mtx[l][i]);

#ifdef DEBUG
    std::cout << "mean  = " << stats.mean() << std::endl;
    std::cout << "        "  << coordm[i] << std::endl;
    std::cout << "stdev = " << stats.stddev()  << std::endl;
#endif

    cstdev[i] = stats.stddev();
  }

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<(3*COORDIM); ++j)
      hca(i,j) = hca(i,j) / (cstdev[i]*cstdev[j]);
  */

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=i+1; j<(3*COORDIM); ++j)
      if (hca(i,j) != hca(j,i))
        std::cout << i << " " << j << " " <<
          hca(i,j) << " ERROR" << std::endl;;

  arma::vec eigval = arma::zeros<arma::mat>(3*COORDIM);
  arma::mat eigvec = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);

  arma::eig_sym(eigval, eigvec, hca);

  double totval = 0.0e0;
  for (int i=0; i<(3*COORDIM); ++i)
    totval += eigval(i);

  int j = 1;
  double totvar = 0.0e0;
  for (int i=(3*COORDIM-1); i>=0; --i)
  {
    if (j <= PARAMDIM)
      totvar += 100.0e0*(eigval(i)/totval);
    ++j;

#ifdef DEBUG
    std::cout << i+1 << " ==> " << 100.0e0*(eigval(i)/totval)
      << " ==> " << eigval(i) <<  std::endl;
#endif

  }
  std::cout << "PARAMDIM eigenvalues: " << totvar << std::endl;

  arma::mat hcai = arma::zeros<arma::mat>(3*COORDIM,3*COORDIM);
  hcai = hca.i();

#ifdef DEBUG
  std::cout << hca * hcai ;
#endif

  // and so on ...
  arma::mat paramm = arma::zeros<arma::mat>(PARAMDIM);
  arma::mat hcap = arma::zeros<arma::mat>(3*COORDIM,PARAMDIM);

  coordm.fill(0.0e0);
  sum = 1.0e0;

  for (int l=0; l<num_of_ent; ++l)
  {
    sum += 1.0e0;
    for (int i=0; i<(3*COORDIM); ++i)
      coordm(i) += (coord_mtx[l][i]-coordm(i))/sum;

    for (int i=0; i<PARAMDIM; ++i)
      paramm(i) += (param_mtx[l][i]-paramm(i))/sum;

    for (int i=0; i<(3*COORDIM); ++i)
    {
      for (int j=0; j<PARAMDIM; ++j)
      {
        hcap(i,j) += ((coord_mtx[l][i] - coordm(i))*
                      (param_mtx[l][j] - paramm(j))-
                      (sum-1.0e0)*hcap(i,j)/sum)/(sum-1.0e0);
      }
    }
  }

  /* correlation matrix
  double pstdev[PARAMDIM];
  for (int i=0; i<PARAMDIM; ++i)
  {
    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(param_mtx[l][i]);

#ifdef DEBUG
    std::cout << "mean   = " << stats.mean() << std::endl;
    std::cout << "         "  << paramm(i) << std::endl;
    std::cout << "stdev  = " << stats.stddev()  << std::endl;
#endif

    pstdev[i] = stats.stddev();
  }

  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<PARAMDIM; ++j)
      hcap(i,j) = hcap(i,j) / (cstdev[i]*pstdev[j]);
  */

  arma::mat cmtx = arma::zeros<arma::mat>(PARAMDIM,3*COORDIM);

  for (int i=0; i<PARAMDIM; ++i)
    for (int l=0; l<(3*COORDIM); ++l)
      for (int m=0; m<(3*COORDIM); ++m)
        cmtx(i,l) += hcai(l,m) * hcap (m,i);

#ifdef DEBUG
  std::cout << "C matrix: " << std::endl;
  std::cout << cmtx;
#endif

  arma::mat q = arma::zeros<arma::mat>(PARAMDIM);

  for (int i=0; i<PARAMDIM; ++i)
  {
    q(i) = paramm(i);
    for (int l=0; l<(3*COORDIM); ++l)
      q(i) -= cmtx(i,l)*coordm[l];
  }

#ifdef DEBUG
  std::cout << "Q vector: " << std::endl;
  for (int i=0; i<PARAMDIM; ++i)
    std::cout << q(i) << std::endl;
#endif

  arma::mat a = arma::zeros<arma::mat>((3*COORDIM),(3*COORDIM));
  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<(3*COORDIM); ++j)
      a(i,j) = eigvec(i,j)/sqrt(eigval(i));

  arma::mat k = arma::zeros<arma::mat>(3*COORDIM);
  for (int i=0; i<(3*COORDIM); ++i)
    for (int j=0; j<(3*COORDIM); ++j)
      k(i) += a(i,j)*coordm(j);

  //test back
  arma::running_stat<double> chi2stats;
  arma::running_stat<double> pc[PARAMDIM];
  for (int l=0; l<num_of_ent; ++l)
  {

    for (int i=0; i<PARAMDIM; ++i)
    {
      double p = q(i);
      for (int k=0; k<(3*COORDIM); ++k)
        p += cmtx(i,k)*coord_mtx[l][k];

      pc[i](fabs(p - param_mtx[l][i])/(fabs(p + param_mtx[l][i])/2.0));
    }

    /* chi**2 */
    double chi2 = 0.0e0;
    for (int i=0; i<(3*COORDIM)-PARAMDIM; ++i)
    {
      double v = k(i);

      for (int j=0; j<(3*COORDIM); ++j)
        v += a(i,j) * coord_mtx[l][j];

      chi2 += (v*v);
    }
    chi2stats(chi2);
  }
  std::cout << "chi2 mean   = " << chi2stats.mean() << std::endl;
  std::cout << "chi2 stdev  = " << chi2stats.stddev()  << std::endl;
  std::cout << "chi2 min    = " << chi2stats.min() << std::endl;
  std::cout << "chi2 max    = " << chi2stats.max() << std::endl;

  for (int i=0; i<PARAMDIM; ++i)
  {
    std::cout << pc[i].mean() << " " << pc[i].stddev() << std::endl;

    arma::running_stat<double> stats;

    for (int l=0; l<num_of_ent; ++l)
      stats(param_mtx[l][i]);

    std::cout << "  mean   = " << stats.mean() << std::endl;
    std::cout << "  stdev  = " << stats.stddev()  << std::endl;
    std::cout << "  min    = " << stats.min() << std::endl;
    std::cout << "  max    = " << stats.max() << std::endl;
  }

  for (int i = 0; i < num_of_ent; ++i)
  {
    delete(coord_mtx[i]);
    delete(param_mtx[i]);
  }
  delete(coord_mtx);
  delete(param_mtx);

  return 0;
}
<|endoftext|>