/*
 * File:   BatchProcessing.cpp
 * Author: jorge
 *
 * Created on 8 de octubre de 2013, 17:21
 */

#include "BatchProcessing.h"
#include <QFileDialog>

using namespace std;

BatchProcessing::BatchProcessing()
{
    widget.setupUi(this);
    
    working = false;
    finished = false;
    force_stop = false;
    
    number_of_threads = 10;
    
    calculateStereo = true;
    calculate3d = true;
    calculateBackgroundSubtraction = true;
    
    buffer.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    update_viewer = false;
    processed_ = false;
    
    current_iteration_ = 1;
    
    stop_ = false;
    new_pose_ = false;
    
    show_samples_ = false;
    
    current_model_height_ = 1.7;
    
    c_position_ = makePoint(-2,1,-2);
    c_look_at_ = makePoint(0,0,0);
    c_orientation_ = makePoint(0,1,0);
}

BatchProcessing::~BatchProcessing() {
}

bool BatchProcessing::stereo(string left,string right,string disparity, string normalized_disparity, string filestorage)
{
    /*int min_disp = 1;
    int disp = 5;
    int SAD = 3;
    int SGMp1 = 72;
    int SGMp2 = 228;
    int disp12_max_diff = 0;
    int pre_filter_cap = 64;
    int unique = 0;
    int speckle_window = 164;//164;
    int speckle_range = 1;*/
    
    /*int min_disp = 5;
    int disp = 5;
    int SAD = 3;
    int SGMp1 = 0;
    int SGMp2 = 128;
    int disp12_max_diff = 0;
    int pre_filter_cap = 64;
    int unique = 0;
    int speckle_window = 164;
    int speckle_range = 1;
*/
    int min_disp = 1;
    int disp = 7;
    int SAD = 3;
    int SGMp1 = 72;
    int SGMp2 = 228;
    int disp12_max_diff = 0;
    int pre_filter_cap = 64;
    int unique = 0;
    int speckle_window = 164;//164;
    int speckle_range = 1;
    
    Stereo m_stereo;
    
    cv::Mat disp_img;
    
    m_stereo.sgbmKitti(left, right, &disp_img,
            min_disp, disp, SAD, SGMp1, SGMp2, disp12_max_diff, pre_filter_cap,
            unique, speckle_window, speckle_range);
    
    cv::Mat auxShow(disp_img.rows, disp_img.cols, CV_8UC1);
    cv::Mat auxDisp(disp_img.rows, disp_img.cols, CV_8UC1);
    disp_img.convertTo(auxDisp, CV_8UC1);
    cv::normalize(auxDisp, auxShow, 0, 255, CV_MINMAX);

    try
    {
        cv::imwrite(normalized_disparity,auxShow);
        cv::imwrite(disparity,disp_img);
        
        cout<<"saved: "<<normalized_disparity<<endl;
        cout<<"saved: "<<disparity<<endl;
        
        cv::FileStorage fs(filestorage, cv::FileStorage::WRITE);
        fs << "disparity_image" << disp_img;
        cout<<"saved: "<<filestorage<<endl;
        
    }catch(runtime_error& ex)
    {
        cout<< "Exception converting image to PNG format: " << ex.what() <<endl;
        return false;
    }
    
    return true;
}

bool BatchProcessing::calculatePointCloud(string disparity_filestorage,string color_image,string pcd_cloud)
{  
    //Create the filestorage reading object
    cv::FileStorage fs(disparity_filestorage, cv::FileStorage::READ);
    
    cv::Mat disp_img;
    //Load disparity image (true values)
    fs["disparity_image"] >> disp_img;
    
    //Create a point cloud
    pcl::PointCloud<pcl::PointXYZRGB> cloud(disp_img.cols,disp_img.rows);

    // Fill in the cloud data
    cloud.is_dense = false;
       
    int cx = 690;
    int cy = 247.1364;
    double fx = 981.2178;
    double fy = 975.8994;
    double B = 0.54;

    //Load left color image to copy the color to the pcl point cloud
    cv::Mat color = cv::imread(color_image, CV_LOAD_IMAGE_COLOR);
    
    //For each pixel calculate the xyz position
    for(int row=0;row<disp_img.rows;row++)
    {
        for(int col=0;col<disp_img.cols;col++)
        {
            double u = col - cx;
            double v = row - cy;

            pcl::PointXYZRGB p;
                    
            if(!color_image.empty())
            {
                cv::Vec3b pc = color.at<cv::Vec3b>(row,col);

                p.r=pc[2];
                p.g=pc[1];
                p.b=pc[0];
            }

            //p.data
            double z = fx*B/disp_img.at<float>(row,col);
            
            //cout<<"dsp "<<disp_img.at<float>(row,col)<<endl;
            
            p.z = z;
            p.x = -u*z/fx;
            p.y = -v*z/fy;
            
            
            cloud.at(col,row) = p;
        }
    }
    
    //Save pcd file
    pcl::io::savePCDFileASCII (pcd_cloud, cloud);
    cout<<"saved: "<<pcd_cloud<<endl;
    
    return true;
}



bool BatchProcessing::filterPointCloud(string cloud_file,string background, string filtered_cloud)
{
    //Load non filtered point cloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
   
    if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (cloud_file, *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read pcd file \n");
        return false;
    }
    
    pcl::PointXYZRGB pinf;
    pinf.x = pinf.y = pinf.z = std::numeric_limits<double>::infinity();
    
    if(useRange)
    {
        pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZRGB> ());
        
        double xmax = 1e6;
        double ymax = 1e6;
        double zmax = 18;
        double xmin = -1e6;
        double ymin = -1e6;
        double zmin = 0;
        
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::GT, zmin)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::LT, zmax)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::GT, xmin)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::LT, xmax)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::GT, ymin)));
        range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::LT, ymax)));

        // build the filter
        pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem(range_cond);
        condrem.setInputCloud(cloud);
        
        //pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
       
        condrem.setKeepOrganized (true);
        condrem.filter(*cloud);
    }
    
    if(useGroundPlaneEstimation)
    {
        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
        // Create the segmentation object
        
        pcl::SACSegmentation<pcl::PointXYZRGB> seg;
        // Optional
        seg.setOptimizeCoefficients (true);
        // Mandatory
        seg.setModelType (pcl::SACMODEL_PLANE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setMaxIterations (150);
        seg.setDistanceThreshold (0.05);
        
        seg.setInputCloud(cloud);
        seg.segment(*inliers,*coefficients);

        for(unsigned int i=0;i<inliers->indices.size();i++)
            cloud->at(inliers->indices[i]) = pinf;
    }
    
    if(useBackgroundSubtraction)
    {
        //Load background mask
        cv::Mat background_mask = cv::imread(background, CV_LOAD_IMAGE_GRAYSCALE);
        
        for(int row=0;row<background_mask.rows;row++)
            for(int col=0;col<background_mask.cols;col++)
                if(!background_mask.at<uchar>(row,col))
                    cloud->at(col,row) = pinf;
    }

    pcl::io::savePCDFileASCII (filtered_cloud,*cloud);
    cout<<"saved: "<<filtered_cloud<<endl;
    
    return true;
}

void BatchProcessing::doProcessing()
{
    mtx_.lock();
    
    working = true;
    finished = false;
    force_stop = false;
    
    unsigned int _start_frame = start_frame;
    unsigned int _end_frame = end_frame;
    
    mtx_.unlock();
    
    boost::filesystem::path main_folder(folder);
    
    boost::filesystem::path left_path(main_folder / "image_02" / "data");
    boost::filesystem::path right_path(main_folder / "image_03" / "data");
    
    boost::filesystem::path disparity_normalized_path(main_folder / "image_disparity_normalized" / "data");
    boost::filesystem::path disparity_path(main_folder / "image_disparity" / "data");
    boost::filesystem::path disparity_filestorage_path(main_folder / "image_disparity_filestorage" / "data");
    
    //Create the disparity folder
    boost::filesystem::create_directories(disparity_path);

    //Create the normalized disparity folder
    boost::filesystem::create_directories(disparity_normalized_path);

    //Create the disparity filestorage folder
    boost::filesystem::create_directories(disparity_filestorage_path);

    //Get iterators
    boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
    boost::filesystem::directory_iterator itr(left_path);
    
    std::vector<boost::filesystem::path> paths;
    
    //Count the number of folders
    for(;itr != end_itr;itr++)
    {
        if(boost::filesystem::is_regular_file(itr->status()))
            paths.push_back(itr->path());
    }
        
    std::sort(paths.begin(),paths.end());
    
    mtx_.lock();
    current_frame = 0;
    mtx_.unlock();
        
    //start processing for all files in the image_00/data folder
    for(unsigned int i=_start_frame;i<_end_frame;)
    {
        if(i>=paths.size())
            break;
        
        boost::thread_group group;
        
        for(unsigned int j=0;j<number_of_threads;j++)
        {
            if(i>=_end_frame || i>=paths.size())
                break;
            
            //Left image file
            string left = paths[i].string();     
            //Right image file
            string right = (right_path/paths[i].filename()).string();
            //Disparity image file
            string disparity = (disparity_path/paths[i].filename()).string();
            //Normalized disparity image file
            string disparity_normalized = (disparity_normalized_path/paths[i].filename()).string();
            //Filestorage
            string disparity_filestorage = (disparity_filestorage_path/paths[i].filename()).string();
            disparity_filestorage.replace(disparity_filestorage.find("."),string::npos,".yml");

            //Calculate stereo and save files
            group.create_thread(boost::bind(&BatchProcessing::stereo,this,left,right,disparity,disparity_normalized,disparity_filestorage));
            i++;
        }
        
        group.join_all();
        
        mtx_.lock();
        current_frame = i;
        mtx_.unlock();
    }
    
    cout<<"Done"<<endl;
    
    mtx_.lock();
    working = false;
    finished = true;
    mtx_.unlock();
}

void BatchProcessing::update()
{
    mtx_.lock();
    widget.progressBar->setValue(current_frame + 1);
    mtx_.unlock();
}

void BatchProcessing::update_mfp()
{
    mtx_mfp_.lock();
    widget.progressBarMFP->setValue(current_folder_progress);
    mtx_mfp_.unlock();
}

void BatchProcessing::startProcessing() {
    
    start_frame = widget.startFrame->text().toInt();
    end_frame = widget.endFrame->text().toInt();
    
    widget.progressBar->setMinimum(start_frame);
    widget.progressBar->setMaximum(end_frame);
    
    folder = widget.dataFolder->text().toStdString();
    
    cout<<"Start processing, from: " << start_frame << " to: " << end_frame <<endl;
    cout<<"Processing folder: " << folder << endl;
    
    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(update()));
    timer->start(1000);
    
    process = new boost::thread(&BatchProcessing::doProcessing,this);
}


bool sortContoursLength(const vector<cv::Point>& c1,const vector<cv::Point>& c2)
{
    return c1.size()>c2.size();
}

void BatchProcessing::singleFolderProcessing(const boost::filesystem::path& folder)
{
    boost::filesystem::path left_path(folder / "image_00" / "data");
    boost::filesystem::path left_color_path(folder / "image_02" / "data");
    boost::filesystem::path right_path(folder / "image_01" / "data");
    
    singleFolderProcessing(folder,left_path,right_path,left_color_path);
}

void BatchProcessing::singleFolderProcessing(const boost::filesystem::path& folder,
                                                const boost::filesystem::path& left_path, 
                                                const boost::filesystem::path& right_path, 
                                                const boost::filesystem::path& left_color_path)
{
    
    boost::filesystem::path disparity_normalized_path(folder / "image_disparity_normalized" / "data");
    boost::filesystem::path disparity_path(folder / "image_disparity" / "data");
    boost::filesystem::path disparity_filestorage_path(folder / "image_disparity_filestorage" / "data");
    
    boost::filesystem::path pcd_path(folder / "pcd" / "data");
    boost::filesystem::path pcd_filtered_path(folder / "pcd_filtered" / "data");
    
    boost::filesystem::path background_masks_path(folder / "background_masks" / "data");
    
    //cout<<"doing folder "<<folder.string()<<endl;
    
    //Create the background subtractor object for this folder
    cv::BackgroundSubtractorMOG2 background_subtractor = cv::BackgroundSubtractorMOG2(20,6.0,false);
    
    try
    {
        if(calculateStereo)
        {
            //Create the disparity folder
            boost::filesystem::create_directories(disparity_path);

            //Create the normalized disparity folder
            boost::filesystem::create_directories(disparity_normalized_path);
            
            //Create the disparity filestorage folder
            boost::filesystem::create_directories(disparity_filestorage_path);
        }
        
        if(calculate3d)
        {
            //Create the normalized pcd folder
            boost::filesystem::create_directories(pcd_path);
        }
        
        if(calculateBackgroundSubtraction)
        {
            //Create the background masks folder
            boost::filesystem::create_directories(background_masks_path);
        }
        
        if(doFiltering)
        {
            //Create the point cloud filtered folder
            boost::filesystem::create_directories(pcd_filtered_path);
        }
        
        //Get iterators
        boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
        boost::filesystem::directory_iterator itr(left_path);
    
        std::vector<boost::filesystem::path> paths;
    
        //Count the number of folders
        for(;itr != end_itr;itr++)
        {
            if(boost::filesystem::is_regular_file(itr->status()))
                paths.push_back(itr->path());
        }
        
        std::sort(paths.begin(),paths.end());
        
        //start processing for all files in the image_00/data folder
        for(unsigned int i=0;i<paths.size();i++)//Do this in multi thread in the future
        {
            if(calculateStereo)
            {
                //Left image file
                string left = paths[i].string();     
                //Right image file
                string right = (right_path/paths[i].filename()).string();
                //Disparity image file
                string disparity = (disparity_path/paths[i].filename()).string();
                //Normalized disparity image file
                string disparity_normalized = (disparity_normalized_path/paths[i].filename()).string();
                //Filestorage
                string disparity_filestorage = (disparity_filestorage_path/paths[i].filename()).string();
                disparity_filestorage.replace(disparity_filestorage.find("."),string::npos,".yml");
                        
                //Calculate stereo and save files
                stereo(left,right,disparity,disparity_normalized,disparity_filestorage);
            }

            if(calculate3d)
            {
                //Filestorage
                string disparity_filestorage = (disparity_filestorage_path/paths[i].filename()).string();
                disparity_filestorage.replace(disparity_filestorage.find("."),string::npos,".yml");
                //Left image color file
                string left_color = (left_color_path/paths[i].filename()).string();
                //Pcd file
                string pcd = (pcd_path/paths[i].filename()).string();
                pcd.replace(pcd.find("."),string::npos,".pcd");

                //Compute 3d with color and save files
                calculatePointCloud(disparity_filestorage,left_color,pcd);
            }

            if(calculateBackgroundSubtraction)
            {
                //Do stuff related to background subtraction
                
                //Left image color file
                string left_color = (left_color_path/paths[i].filename()).string();
                
                //Background mask file
                string background = (background_masks_path/paths[i].filename()).string();
                
                cv::Mat color = cv::imread(left_color, CV_LOAD_IMAGE_COLOR);
                cv::Mat background_mask;
                
                background_subtractor.operator()(color,background_mask,0.02);
                
                cv::medianBlur(background_mask,background_mask, 3);
                        
                //Save raw background mask
                //cv::imwrite(background,background_mask);
                
                vector<vector<cv::Point> > contours;
                vector<cv::Vec4i> hierarchy;
                
                //Get contours
                cv::findContours(background_mask,contours,hierarchy,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_NONE);
                
                //Sort contours by length
                sort(contours.begin(),contours.end(),sortContoursLength);
                
                // Draw main contour or all contours, the main is the first in the list
                cv::Mat drawing = cv::Mat::zeros( background_mask.size(), CV_8U);
                
                for(unsigned int i=0;i<contours.size();i++)//Draw all contours, the first is the person
                    cv::drawContours( drawing, contours, i, 255, CV_FILLED, 8, hierarchy);

                cv::imwrite(background,drawing);
                cout<<"saved: "<<background<<endl;
            }
            
            if(doFiltering)
            {
                //Original point cloud file
                string cloud = (pcd_path/paths[i].filename()).string();
                cloud.replace(cloud.find("."),string::npos,".pcd");
                        
                //Filtered point cloud file
                string filtered = (pcd_filtered_path/paths[i].filename()).string();
                filtered.replace(filtered.find("."),string::npos,".pcd");
                
                //Background mask file
                string background = (background_masks_path/paths[i].filename()).string();
                
                filterPointCloud(cloud,background,filtered);
            }
            
        }
    }catch(boost::filesystem::filesystem_error error)
    {
        cout<<"Error: "<<error.what()<<endl;
    }
    
}

bool BatchProcessing::doMFP()
{
    //Main folder
    boost::filesystem::path main_folder(widget.mfpFolder->text().toStdString());

    //Check if valid
    if (!boost::filesystem::exists(main_folder)) 
        return false;
  
    //Get iterators
    boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
    boost::filesystem::directory_iterator itr(main_folder);
    
    std::vector<boost::filesystem::path> paths;
    
    //Count the number of folders
    for(;itr != end_itr;itr++)
    {
        if(boost::filesystem::is_directory(itr->status()))
            paths.push_back(itr->path());
    }
    
    std::sort(paths.begin(),paths.end());
    
    boost::thread_group group;//Thread group
    
    mtx_mfp_.lock();
    current_folder_progress = 0;
    mtx_mfp_.unlock();
        
    //While not at the end
    for(unsigned int i=0;i<paths.size();)
    {
        //At threads
        for(unsigned int t=0; t<number_of_threads && i!=paths.size(); ++t)
        {
            //Add create thread and add to thread group
            group.create_thread(boost::bind(&BatchProcessing::singleFolderProcessing,this,paths[i++]));
        }
        
        group.join_all();
        
        mtx_mfp_.lock();
        current_folder_progress = (double)i/(double)paths.size()*100.;
        mtx_mfp_.unlock();
    }
    
    widget.startMFP->setEnabled(true);
    
    cout<<"All done"<<endl;
    
    return true;
}
        
void BatchProcessing::startMFP()
{
    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(update_mfp()));
    timer->start(100);
    
    widget.startMFP->setEnabled(false);
    
    calculateStereo = widget.computeStereo->isChecked();
    calculateBackgroundSubtraction = widget.computeBackgroundSubtraction->isChecked();
    calculate3d = widget.computePointcloud->isChecked();
    
    doFiltering = widget.doFiltering->isChecked();
    useBackgroundSubtraction = widget.useBackgroundSubtraction->isChecked();
    useGroundPlaneEstimation = widget.useGroundPlaneEstimation->isChecked();
    useRange = widget.useRange->isChecked();
    
    mfprocess = new boost::thread(&BatchProcessing::doMFP,this);
}

bool BatchProcessing::doVIS(string pcd_file)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (pcd_file, *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return false;
    }

    doVIS(cloud);
    
    return true;
}

bool BatchProcessing::doVIS(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0.4, 0.4, 0.4);
    
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    
    viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
    
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
    
    viewer->close();
    
    return true;
}

void BatchProcessing::startVisualization()
{
    string pcd_file = widget.visFile->text().toStdString();
    visprocess = new boost::thread(boost::bind(static_cast<bool (BatchProcessing::*)(string)>(&BatchProcessing::doVIS),this,pcd_file));
}


void BatchProcessing::openDir() {
    // kitti dir /home/jorge/Escritorio/kitti/2012_object/training
    QString dirName = QFileDialog::getExistingDirectory(this, tr("Open KITTI Directory"), "", QFileDialog::ShowDirsOnly);
    widget.dataFolder->setText(dirName);
}

void BatchProcessing::stopProcessing()
{
    cout<<"Stop processing"<<endl;
    
    mtx_.lock();
    
    if(working)
    {
        force_stop=true;
        delete timer;
    }
    
    mtx_.unlock();
}

void BatchProcessing::readCloud(string cloud_file,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Load non filtered point cloud
    if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (cloud_file, *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read pcd file \n");
        return;
    }
}

void BatchProcessing::startPoseEstimation()
{
    save_pose_text_ = widget.savePoseText->isChecked();
    save_screenshot_ = widget.saveScrenShot->isChecked();
    
    interactive_ = widget.interactive->isChecked();
    if(interactive_)
    {
        current_iteration_ = widget.startFramePoseEstimation->value();
        current_model_height_ = widget.modelHeight->value();
    }else
        current_iteration_ = 0;
    
    trial_folder =  trial_folder / widget.singleTrialPath->text().toStdString();
            
    //Start visualization thread
    visualization_process = new boost::thread(boost::bind(&BatchProcessing::visualization,this)); 
    
    //Start tests thread
    generic_process = new boost::thread(boost::bind(&BatchProcessing::poseEstimatorWorker,this)); 
}

void BatchProcessing::startBatchPoseEstimation()
{
    save_pose_text_ = widget.savePoseText->isChecked();
    save_screenshot_ = widget.saveScrenShot->isChecked();
    use_external_list_ = widget.useExternalList->isChecked();
    
    if(use_external_list_)
        external_list_path_ = widget.extrenalListPath->text().toStdString();
            
    main_folder = main_folder / widget.mainDataFolder->text().toStdString();
    
    //Start visualization thread
    visualization_process = new boost::thread(boost::bind(&BatchProcessing::visualization,this)); 
    
    //Start tests thread
    generic_process = new boost::thread(boost::bind(&BatchProcessing::batchPoseEstimatorWorker,this)); 
}

void BatchProcessing::readXMLlist(std::string list_path,std::vector<boost::filesystem::path>& paths,std::vector<double>& heights)
{
    rapidxml::xml_document<> doc;
    // Read the xml file into a vector
    std::ifstream data(list_path.c_str());
    
    if(!data.good())
    {
        cerr<<"Error opening "<<list_path<<endl;
        cerr<<"Unable to load xml trial list."<<endl;
        return;
    }
        
    std::vector<char> buffer((std::istreambuf_iterator<char>(data)), std::istreambuf_iterator<char>());
    buffer.push_back('\0');
    // Parse the buffer using the xml file parsing library into doc 
    doc.parse<0>(&buffer[0]);
    
    // Find our root node
    rapidxml::xml_node<>* root_node = doc.first_node("list");
    
    //Get body segment ratios
    for(rapidxml::xml_node<>*trial = root_node->first_node("trial");trial;trial = trial->next_sibling())
    {
        std::string trial_path = trial->first_node("path")->value();
        double height = atof(trial->first_node("model_height")->value());
        std::string status = trial->first_node("status")->value();
        std::string comment = trial->first_node("coment")->value();
        
        if(status=="done")
        {
            cout<<"trial already processed, jumping ("<<trial_path<<")"<<endl;
            continue;
        }
        else if(status=="redo")
        {
            cout<<"trial marker to redo, processing ("<<trial_path<<")"<<endl;
            paths.push_back(boost::filesystem::path(trial_path));
            heights.push_back(height);
        }
        else if(status=="not done")
        {
            cout<<"trial marked for processing ("<<trial_path<<")"<<endl;
            paths.push_back(boost::filesystem::path(trial_path));
            heights.push_back(height);
        }
        else
        {
            cerr<<"Unknown trial status ("<<trial_path<<") status: "<<status<<endl;
            cerr<<"jumping"<<endl;
            continue;
        }
    }
}

void BatchProcessing::batchPoseEstimatorWorker()
{
    //List all trial folders in the main folder
    //Do poseEstimationWorker for each one
    
    if(use_external_list_)
    {
        std::vector<boost::filesystem::path> paths;
        std::vector<double> heights;
        
        readXMLlist(external_list_path_,paths,heights);
        
        //Sort files
        std::sort(paths.begin(),paths.end());
        
        for(uint i=0;i<paths.size();i++)
        {
            interactive_ = false;
            trial_folder = paths[i];
            current_model_height_ = heights[i];
            current_iteration_ = 1;
            cout<<"doing estimation on: "<<trial_folder.string()<<" with height: "<<current_model_height_<<endl;
            
            poseEstimatorWorker();
        }
        
    }else
    {
        //Get iterators
        boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
        boost::filesystem::directory_iterator itr(main_folder);

        std::vector<boost::filesystem::path> paths;

        //Count the number of files
        for(;itr != end_itr;itr++)
        {
            if(boost::filesystem::is_directory(itr->status()))
                paths.push_back(itr->path());
        }

        //Sort files
        std::sort(paths.begin(),paths.end());

        for(uint i=0;i<paths.size();i++)
        {
            interactive_ = false;
            trial_folder = paths[i];
            current_iteration_ = 0;
            cout<<"doing estimation on:"<<trial_folder.string()<<endl;
            
            poseEstimatorWorker();
        }
    }
}

void BatchProcessing::poseEstimatorWorker()
{
    //Load folders
    boost::filesystem::path pcd_filtered_path(trial_folder / "pcd_filtered" / "data");
    boost::filesystem::path detections(trial_folder / "detections_text" / "data");
    
    boost::filesystem::path pose_text_folder(trial_folder / "pose_text" / "data");
    boost::filesystem::path pose_XML_folder(trial_folder / "pose_xml" / "data");
    
    //Get iterators
    boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
    boost::filesystem::directory_iterator itr(pcd_filtered_path);
    
    std::vector<boost::filesystem::path> paths;
    
    //Count the number of files
    for(;itr != end_itr;itr++)
    {
        if(boost::filesystem::is_regular_file(itr->status()))
            paths.push_back(itr->path());
    }
    
    //Sort files
    std::sort(paths.begin(),paths.end());
    
    if(paths.size()==0)
    {
        cerr<<"No files to process in: "<<trial_folder.string()<<endl;
        return;
    }
    
    //Pose estimation class
    PoseEstimation pose_estimation;
    
    pose_estimation.model_.height_ = current_model_height_;
    
    int count = 0;
    
    if(save_pose_text_)
    {
        boost::filesystem::create_directories(pose_text_folder);
        string pose_version_file = (pose_text_folder/"version.txt").string();
        pose_estimation.writeVersion(pose_version_file);
        
        boost::filesystem::create_directories(pose_XML_folder);
    }
    
    while(!stop_)
    {
        mtx_.lock();
        if(!processed_)
        {
            current_iteration_ = max(current_iteration_,(uint)0);
            current_iteration_ = min(current_iteration_,(uint)(paths.size()-1));

            //Load non filtered point cloud
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
            
            current_file_name = (paths[current_iteration_].filename()).string();
            
            if(count>0)
            {
                draw.lock();
                viewer->removeShape("status");
                viewer->addText((boost::format("Loading: %1% ...")%paths[current_iteration_].string()).str(),5,20,16,1.0,0.3,0.,"status");
                draw.unlock();
            }
            
            cout<<"loading: "<<paths[current_iteration_].string()<<endl;
            if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (paths[current_iteration_].string(), *cloud) == -1) //* load the file
            {
                PCL_ERROR ("Couldn't read pcd file \n");
                break;
            }
            
            draw.lock();
            viewer->removeShape("status");
            viewer->addText((boost::format("Processing: %1% ...")%paths[current_iteration_].string()).str(),5,20,16,1.0,0.3,0.,"status");
            draw.unlock();
            
            cout<<"processing: "<<paths[current_iteration_].string()<<endl;
            pose_estimation.segmentPedestrian(cloud);
            cout<<"getting pose"<<endl;
            pose_estimation.getPose();
            
            if(save_pose_text_)
            {
                string pose_text_file = (pose_text_folder/current_file_name).string();
                pose_text_file.replace(pose_text_file.find("."),string::npos,".txt");
                pose_estimation.savePoseText(pose_text_file);
                
                string pose_XML_file = (pose_XML_folder/current_file_name).string();
                pose_XML_file.replace(pose_XML_file.find("."),string::npos,".xml");
                pose_estimation.savePoseXML(pose_XML_file);
                
                cout<<"saved pose data to: "<<pose_text_file<<" and "<<pose_XML_file<<endl;
            }
            
            draw.lock();
            c_position_ = pose_estimation.model_.head.start_ - makePoint(-1,0,3);
            c_look_at_ = pose_estimation.model_.center_torso.start_;
            c_orientation_ = makePoint(0,1,0);
            
            viewer->removeShape("status");
            viewer->addText(string("Drawing pose ..."),5,20,16,0.,0.,1.,"status");
            draw.unlock();
            
            cout<<"drawing pose"<<endl;
            drawPose(pose_estimation,pose_estimation.model_);
            
            draw.lock();
            viewer->removeShape("status");
            viewer->addText((boost::format("Ready: %1%")%paths[current_iteration_].string()).str(),5,20,16,0.,1.,0.,"status");
            draw.unlock();
            
            draw.lock();
            viewer->removeShape("samples_info");
            if(show_samples_)
                viewer->addText(string("showing samples"),5,40,12,0.,1.,0.,"samples_info");
            else
                viewer->addText(string("not showing samples"),5,40,12,0.8,0.,0.,"samples_info");
            draw.unlock();
            
            cout<<"done"<<endl;
            processed_ = true;
            count++;
        }
        mtx_.unlock();
        
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
        
        if(!interactive_)
        {
            current_iteration_++;
            processed_ = false;
            
            //Finish with this trial
            if(current_iteration_>=paths.size())
                return;
        }
    }
}

void BatchProcessing::setCameraPose(pcl::PointXYZRGB c_position,pcl::PointXYZRGB c_look_at,pcl::PointXYZRGB c_orientation)
{
    vtkSmartPointer<vtkRendererCollection> rens_ = viewer->getRendererCollection();
    
    rens_->InitTraversal ();
    vtkRenderer* renderer = NULL;
    while ((renderer = rens_->GetNextItem ()) != NULL)
    {
        // Modify all renderer's cameras
        vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
        cam->SetPosition (c_position.x, c_position.y, c_position.z);
        cam->SetFocalPoint (c_look_at.x,c_look_at.y,c_look_at.z);
        cam->SetViewUp (c_orientation.x,c_orientation.y,c_orientation.z);
    }
}

void BatchProcessing::drawGrid(pcl::PointXYZRGB start_point,int number_cells,double cell_size,double r,double g,double b,string id)
{
    int cellsX = number_cells;
    int cellsZ = number_cells;
    
    double dx=cell_size;
    double dz=cell_size;
    
    string id_l;
    
    for(int ix=-cellsX;ix<=cellsX;ix++)
    {
        pcl::PointXYZRGB p1 = makePoint(ix*dx,0,-cellsZ*dz);
        pcl::PointXYZRGB p2 = makePoint(ix*dx,0,+cellsZ*dz);
        
        p1 = p1 + start_point;
        p2 = p2 + start_point;
        
        id = id_l + (boost::format("gx%1%")%ix).str();
        viewer->addLine(p1,p2,r,g,b,id);
    }
    
    for(int iz=-cellsZ;iz<=cellsZ;iz++)
    {
        pcl::PointXYZRGB p1 = makePoint(-cellsX*dx,0,iz*dz);
        pcl::PointXYZRGB p2 = makePoint(cellsX*dx,0,iz*dz);
        
        p1 = p1 + start_point;
        p2 = p2 + start_point;
        
        id = id_l + (boost::format("gz%1%")%iz).str();
        viewer->addLine(p1,p2,r,g,b,id);
    }
    
}

void BatchProcessing::drawSimplePose(PoseEstimation& estimator,PedestrianModel& model)
{
    draw.lock();
    
    viewer->removeAllPointClouds();
    viewer->removeAllShapes();

    drawGrid(makePoint(round(model.neck.start_.x),-1.0,round(model.neck.start_.z)),10,0.5,0.8,0.8,0.8,"gird");
    
    viewer->addSphere(model.neck.start_, 0.03, 1.0, 0.0, 0.0, "head");
    viewer->addSphere(model.neck.end_, 0.03, 1.0, 0.5, 0.0, "neck");

    viewer->addSphere(model.torso.end_, 0.05, 1.0, 0.8, 0.0, "waist");

    viewer->addSphere(model.hips.left_, 0.05, 1.0, 0.0, 0.0, "left_hip");
    viewer->addSphere(model.hips.right_, 0.05, 0.0, 0.0, 1.0, "right_hip");
    
    viewer->addSphere(model.shoulders.left_, 0.05, 1.0, 0.0, 0.0, "left_shoulder");
    viewer->addSphere(model.shoulders.right_, 0.05, 0.0, 0.0, 1.0, "right_shoulders");
    
    if(model.left_upper_arm.detected_)
        viewer->addSphere(model.left_upper_arm.end_, 0.05, 1.0, 0.0, 0.0, "left_upper_arm");
    
    if(model.right_upper_arm.detected_)
    viewer->addSphere(model.right_upper_arm.end_, 0.05, 0.0, 0.0, 1.0, "right_upper_arm");
    
    if(model.left_lower_arm.detected_)
        viewer->addSphere(model.left_lower_arm.end_, 0.05, 1.0, 0.0, 0.0, "left_lower_arm");
    
    if(model.right_lower_arm.detected_)
        viewer->addSphere(model.right_lower_arm.end_, 0.05, 0.0, 0.0, 1.0, "right_lower_arm");
    
    if(model.center_torso.detected_)
    {
        viewer->addSphere(model.center_torso.left_, 0.05, 1.0, 0.0, 0.0, "left_center_torso");
        viewer->addSphere(model.center_torso.right_, 0.05, 0.0, 0.0, 1.0, "right_center_torso");
    }
    
    viewer->addSphere(model.left_leg.end_, 0.05, 1.0, 0.0, 0.0, "left_knee");
    viewer->addSphere(model.right_leg.end_, 0.05, 0.0, 0.0, 1.0, "right_knee");

    viewer->addSphere(model.left_foot.end_, 0.05, 1.0, 0.0, 0.0, "left_foot");
    viewer->addSphere(model.right_foot.end_, 0.05, 0.0, 0.0, 1.0, "right_foot");
    
    
    Colormap c("hsv_small");
    c.setColor(1.,1.,1.);
    
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.neck.start_,model.neck.end_,c.r,c.g,c.b,"neck-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.upper_torso.start_,model.upper_torso.end_,c.r,c.g,c.b,"upper-torso-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.torso.start_,model.torso.end_,c.r,c.g,c.b,"torso-line");

    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.left_,model.shoulders.right_,c.r,c.g,c.b,"shoulder-line");
    
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.left_,model.shoulders.right_,c.r,c.g,c.b,"shoulders-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.left_,model.center_torso.left_,c.r,c.g,c.b,"left-shoulder-torso");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.right_,model.center_torso.right_,c.r,c.g,c.b,"right-shoulder-torso");
    
    if(model.left_upper_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_upper_arm.start_,model.left_upper_arm.end_,c.r,c.g,c.b,"left-upper-arm-line");
    
    if(model.right_upper_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_upper_arm.start_,model.right_upper_arm.end_,c.r,c.g,c.b,"right-upper-arm-line");
    
    if(model.left_lower_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_lower_arm.start_,model.left_lower_arm.end_,c.r,c.g,c.b,"left-lower-arm-line");
    
    if(model.right_lower_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_lower_arm.start_,model.right_lower_arm.end_,c.r,c.g,c.b,"right-lower-arm-line");
    
    if(model.center_torso.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.left_,model.center_torso.right_,c.r,c.g,c.b,"center_torso-line");
    
    if(model.center_torso.detected_ && model.hips.detected_)
    {
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.left_,model.hips.left_,c.r,c.g,c.b,"left-torso-hip");
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.right_,model.hips.right_,c.r,c.g,c.b,"right-torso-hip");
    }
    
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.hips.left_,model.hips.right_,c.r,c.g,c.b,"hips-line");

    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_leg.start_,model.left_leg.end_,c.r,c.g,c.b,"left-leg-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_leg.start_,model.right_leg.end_,c.r,c.g,c.b,"right-leg-line");

    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_foot.start_,model.left_foot.end_,c.r,c.g,c.b,"left-foot-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_foot.start_,model.right_foot.end_,c.r,c.g,c.b,"right-foot-line");
    
    viewer->addPointCloud(estimator.cloud_pedestrian_,"pedestrian");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "pedestrian");

    setCameraPose(c_position_,c_look_at_,c_orientation_);
    
    new_pose_=true;
    
    draw.unlock();
    
}

void BatchProcessing::drawPose(PoseEstimation& estimator,PedestrianModel& model)
{
    draw.lock();
    
    viewer->removeAllPointClouds();
    viewer->removeAllShapes();

    drawGrid(makePoint(round(model.neck.start_.x),-1.0,round(model.neck.start_.z)),10,0.5,0.8,0.8,0.8,"gird");
    
    viewer->addSphere(model.neck.start_, 0.03, 1.0, 0.0, 0.0, "head");
    viewer->addSphere(model.neck.end_, 0.03, 1.0, 0.5, 0.0, "neck");

    viewer->addSphere(model.torso.end_, 0.05, 1.0, 0.8, 0.0, "waist");

    viewer->addSphere(model.hips.left_, 0.05, 1.0, 0.0, 0.0, "left_hip");
    viewer->addSphere(model.hips.right_, 0.05, 0.0, 0.0, 1.0, "right_hip");
    
    viewer->addSphere(model.shoulders.left_, 0.05, 1.0, 0.0, 0.0, "left_shoulder");
    viewer->addSphere(model.shoulders.right_, 0.05, 0.0, 0.0, 1.0, "right_shoulders");
    
    if(model.left_upper_arm.detected_)
        viewer->addSphere(model.left_upper_arm.end_, 0.05, 1.0, 0.0, 0.0, "left_upper_arm");
    
    if(model.right_upper_arm.detected_)
    viewer->addSphere(model.right_upper_arm.end_, 0.05, 0.0, 0.0, 1.0, "right_upper_arm");
    
    if(model.left_lower_arm.detected_)
        viewer->addSphere(model.left_lower_arm.end_, 0.05, 1.0, 0.0, 0.0, "left_lower_arm");
    
    if(model.right_lower_arm.detected_)
        viewer->addSphere(model.right_lower_arm.end_, 0.05, 0.0, 0.0, 1.0, "right_lower_arm");
    
    if(model.center_torso.detected_)
    {
        viewer->addSphere(model.center_torso.left_, 0.05, 1.0, 0.0, 0.0, "left_center_torso");
        viewer->addSphere(model.center_torso.right_, 0.05, 0.0, 0.0, 1.0, "right_center_torso");
    }
    
    viewer->addSphere(model.left_leg.end_, 0.05, 1.0, 0.0, 0.0, "left_knee");
    viewer->addSphere(model.right_leg.end_, 0.05, 0.0, 0.0, 1.0, "right_knee");

    viewer->addSphere(model.left_foot.end_, 0.05, 1.0, 0.0, 0.0, "left_foot");
    viewer->addSphere(model.right_foot.end_, 0.05, 0.0, 0.0, 1.0, "right_foot");
    
    //Plot main detected direction
    pcl::PointXYZRGB pstart = model.head.start_;
    pcl::PointXYZRGB pend = model.head.start_ + model.motion_direction*0.5;
    viewer->addLine(pstart,pend,0.0,0.8,1.0,"motion_dir");
        
    pstart = model.neck.end_;
    pend = pstart + model.shoulder_direction*0.7;
    viewer->addLine(pstart,pend,0.0,0.8,1.0,"shoulder_dir");

    if(model.center_torso.detected_)
    {
        pstart = model.center_torso.end_;
        pend = pstart + model.main_direction*0.7;
        viewer->addLine(pstart,pend,0.8,0.3,1.0,"main_dir");
    }
    
    if(show_samples_)
    {
        /*uint number_samples = reinterpret_cast<RotationOnlySampling*>(model.shoulders.sampling_.get())->samples_.rows();
        for(uint i=0;i<number_samples;i++)
        {
            string line_id = (boost::format("shoulder_samples%1%")%i).str();

            Eigen::MatrixXf shoulder_sample;
            shoulder_sample.resize(1,6);
            shoulder_sample = reinterpret_cast<RotationOnlySampling*>(model.shoulders.sampling_.get())->samples_.row(i);

            pcl::PointXYZRGB p1 = makePoint(shoulder_sample(0),shoulder_sample(1),shoulder_sample(2));
            pcl::PointXYZRGB p2 = makePoint(shoulder_sample(3),shoulder_sample(4),shoulder_sample(5));

            viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
        }*/
        
        uint number_samples;
        
        if(model.shoulders.detected_)
        {
            uint number_samples = reinterpret_cast<EllipseShapeSampling*>(model.shoulders.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("shoulder_samples%1%")%i).str();

                Ellipse::Ptr sample = reinterpret_cast<EllipseShapeSampling*>(model.shoulders.sampling_.get())->samples_[i];

                drawEllipse(sample,1.0,0.8,0,line_id);
            }
        }
        
        if(model.left_upper_arm.detected_)
        {
            number_samples = reinterpret_cast<EllipseSampling*>(model.left_upper_arm.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("left_upper_arm_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.left_upper_arm.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.left_upper_arm.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.right_upper_arm.detected_)
        {
            number_samples = reinterpret_cast<EllipseSampling*>(model.right_upper_arm.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("right_upper_arm_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.right_upper_arm.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.right_upper_arm.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }
        
        if(model.left_lower_arm.detected_)
        {
            number_samples = reinterpret_cast<OffCenterEllipseSamplingModified*>(model.left_lower_arm.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("left_lower_arm_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.left_lower_arm.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.left_lower_arm.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.right_lower_arm.detected_)
        {
            number_samples = reinterpret_cast<OffCenterEllipseSamplingModified*>(model.right_lower_arm.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("right_lower_arm_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.right_lower_arm.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.right_lower_arm.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }
        
        if(model.center_torso.detected_)
        {
            number_samples = reinterpret_cast<RotationOnlySampling*>(model.center_torso.sampling_.get())->samples_.rows();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("center_torso_samples%1%")%i).str();

                Eigen::MatrixXf sample;
                sample.resize(1,6);
                sample = reinterpret_cast<RotationOnlySampling*>(model.center_torso.sampling_.get())->samples_.row(i);

                pcl::PointXYZRGB p1 = makePoint(sample(0),sample(1),sample(2));
                pcl::PointXYZRGB p2 = makePoint(sample(3),sample(4),sample(5));

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.neck.detected_)
        {
            number_samples = reinterpret_cast<EllipseSampling*>(model.neck.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("neck_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.neck.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.neck.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }
        
        if(model.upper_torso.detected_)
        {
            number_samples = reinterpret_cast<DistortedSphereSampling*>(model.upper_torso.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("upper_torso_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.upper_torso.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<DistortedSphereSampling*>(model.upper_torso.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.torso.detected_)
        {
            number_samples = reinterpret_cast<DistortedSphereSampling*>(model.torso.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("torso_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.torso.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<DistortedSphereSampling*>(model.torso.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.hips.detected_)
        {
            /*number_samples = reinterpret_cast<RotationOnlySampling*>(model.hips.sampling_.get())->samples_.rows();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("hips_samples%1%")%i).str();

                Eigen::MatrixXf hip_sample;
                hip_sample.resize(1,6);
                hip_sample = reinterpret_cast<RotationOnlySampling*>(model.hips.sampling_.get())->samples_.row(i);

                pcl::PointXYZRGB p1 = makePoint(hip_sample(0),hip_sample(1),hip_sample(2));
                pcl::PointXYZRGB p2 = makePoint(hip_sample(3),hip_sample(4),hip_sample(5));

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }*/
        }

        if(model.left_leg.detected_)
        {
            number_samples = reinterpret_cast<EllipseSampling*>(model.left_leg.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("left_leg_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.left_leg.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.left_leg.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.right_leg.detected_)
        {
            number_samples = reinterpret_cast<EllipseSampling*>(model.right_leg.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("right_leg_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.right_leg.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<EllipseSampling*>(model.right_leg.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }

        if(model.left_foot.detected_)
        {
            number_samples = reinterpret_cast<OffCenterEllipseSampling*>(model.left_foot.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("left_foot_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.left_foot.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<OffCenterEllipseSampling*>(model.left_foot.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }
        
        if(model.right_foot.detected_)
        {
            number_samples = reinterpret_cast<OffCenterEllipseSampling*>(model.right_foot.sampling_.get())->samples_.size();
            for(uint i=0;i<number_samples;i++)
            {
                string line_id = (boost::format("right_foot_samples%1%")%i).str();
                pcl::PointXYZRGB p1 = model.right_foot.start_;
                pcl::PointXYZRGB p2 = reinterpret_cast<OffCenterEllipseSampling*>(model.right_foot.sampling_.get())->samples_[i];

                viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,1.0,0.8,0.0,line_id);
            }
        }
    }
    
    Colormap c("hsv_small");
    c.setColor(1.,1.,1.);
    
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.neck.start_,model.neck.end_,c.r,c.g,c.b,"neck-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.upper_torso.start_,model.upper_torso.end_,c.r,c.g,c.b,"upper-torso-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.torso.start_,model.torso.end_,c.r,c.g,c.b,"torso-line");

    drawEllipse(model.shoulders.ellipse_,c.r,c.g,c.b,"should_ellipse");
    drawEllipse(model.hips.ellipse_,c.r,c.g,c.b,"hips_ellipse");
    
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.left_,model.shoulders.right_,c.r,c.g,c.b,"shoulders-line");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.left_,model.center_torso.left_,c.r,c.g,c.b,"left-shoulder-torso");
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.shoulders.right_,model.center_torso.right_,c.r,c.g,c.b,"right-shoulder-torso");
    
    Colormap c2("hot");
    
    if(model.left_foot.detected_)
    {
        double inc = 64./model.left_foot.minimization.trajectory.size();
        
        for(uint i=0;i<model.left_foot.minimization.trajectory.size();i++)
        {
            c2.setColor(round(inc*i));
            string line_id = (boost::format("left_foot_trajectory_%1%")%i).str();
            viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_foot.start_,model.left_foot.minimization.trajectory[i],c2.r,c2.g,c2.b,line_id);
        }
    }
    
    if(model.left_upper_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_upper_arm.start_,model.left_upper_arm.end_,c.r,c.g,c.b,"left-upper-arm-line");
    
    if(model.right_upper_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_upper_arm.start_,model.right_upper_arm.end_,c.r,c.g,c.b,"right-upper-arm-line");
    
    if(model.left_lower_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_lower_arm.start_,model.left_lower_arm.end_,c.r,c.g,c.b,"left-lower-arm-line");
    
    if(model.right_lower_arm.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_lower_arm.start_,model.right_lower_arm.end_,c.r,c.g,c.b,"right-lower-arm-line");
    
    if(model.center_torso.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.left_,model.center_torso.right_,c.r,c.g,c.b,"center_torso-line");
    
    if(model.center_torso.detected_ && model.hips.detected_)
    {
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.left_,model.hips.left_,c.r,c.g,c.b,"left-torso-hip");
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.center_torso.right_,model.hips.right_,c.r,c.g,c.b,"right-torso-hip");
    }
    
    if(model.hips.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.hips.left_,model.hips.right_,c.r,c.g,c.b,"hips-line");

    if(model.left_leg.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_leg.start_,model.left_leg.end_,c.r,c.g,c.b,"left-leg-line");
    
    if(model.right_leg.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_leg.start_,model.right_leg.end_,c.r,c.g,c.b,"right-leg-line");

    if(model.left_foot.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.left_foot.start_,model.left_foot.end_,c.r,c.g,c.b,"left-foot-line");
    
    if(model.right_foot.detected_)
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (model.right_foot.start_,model.right_foot.end_,c.r,c.g,c.b,"right-foot-line");
    
    
    
    
    /*Eigen::Matrix4f trans_mat; 

    trans_mat <<    1,        0,        0,  1, 
                    0,        1,        0,  0, 
                    0,        0,        1,  0, 
                    0,        0,        0,  1; */

    //pcl::transformPointCloud(*estimator.cloud_knees_,*estimator.cloud_knees_,trans_mat); 
    //pcl::transformPointCloud(*estimator.cloud_top_body_,*estimator.cloud_top_body_,trans_mat); 

    //viewer->addPointCloud(estimator.cloud_knees_,"knees");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "knees");
    
    viewer->addPointCloud(estimator.cloud_pedestrian_,"pedestrian");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "pedestrian");

    //viewer->addPointCloud(estimator.cloud_top_body_,"top_body");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "top_body");
    
    //viewer->addPointCloud(estimator.cloud_bellow_knees_,"bellow_knees");
    //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "bellow_knees");
    
    viewer->addPointCloud(estimator.cloud_accumulation_,"parts");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "parts");

    viewer->addPointCloud(model.head.cloud_,"head");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "head");

    viewer->addPointCloud(model.shoulders.cloud_,"shoulders");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "shoulders");

    viewer->removeShape("p1");
    viewer->addText((boost::format("height: %1% m")%model.height_).str(),5,55,12,0.,0.,0.,"p1");
    
    viewer->removeShape("p2");
    viewer->addText((boost::format("height measured: %1% m")%model.measured_height_).str(),5,70,12,0.,0.,0.,"p2");
    
    viewer->removeShape("p3");
    viewer->addText((boost::format("velocity: %1% m/s")%model.main_velocity).str(),5,85,12,0.,0.,0.,"p3");
    
    setCameraPose(c_position_,c_look_at_,c_orientation_);
    
    new_pose_=true;
    
    draw.unlock();
    
}

void BatchProcessing::drawEllipse(Ellipse::Ptr ellipse,double r,double g,double b,string id)
{
    Eigen::Vector3f p1e;
    Eigen::Vector3f p2e;
    pcl::PointXYZRGB p1;
    pcl::PointXYZRGB p2;
    
    for(int i=0;i<ellipse->points_.rows()-1;i++)
    {
        p1e = ellipse->points_.row(i);
        p2e = ellipse->points_.row(i+1);
        
        p1 = makePoint(p1e);
        p2 = makePoint(p2e);
        
        string seg_id = (boost::format("%1%_%2%")%id%i).str();
        viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,r,g,b,seg_id);
    }
    
    int iend=ellipse->points_.rows()-1;
    
    p1e = ellipse->points_.row(iend);
    p2e = ellipse->points_.row(0);
    
    p1 = makePoint(p1e);
    p2 = makePoint(p2e);

    string seg_id =  (boost::format("%1%_%2%")%id%iend).str();
    viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB > (p1,p2,r,g,b,seg_id);
    
    //pcl::PointXYZRGB u = ellipse->center_ + makePoint(ellipse->u_)*0.5;
    //pcl::PointXYZRGB v = ellipse->center_ + makePoint(ellipse->v_)*0.5;
    //pcl::PointXYZRGB center = ellipse->center_;
    
    //viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB >(center,u,1.0,0.0,0.0,(boost::format("%1%_%2%")%id%"u").str());
    //viewer->addLine<pcl::PointXYZRGB, pcl::PointXYZRGB >(center,v,0.0,1.0,0.0,(boost::format("%1%_%2%")%id%"v").str());
}

void BatchProcessing::keyboardHandler(const pcl::visualization::KeyboardEvent& event)
{
    //if(draw.try_lock())
    //{
        if(event.keyDown())
        {
            if(event.getKeySym()=="Right")
            {
                mtx_.lock();
                current_iteration_++;
                processed_=false;
                mtx_.unlock();

            }else if(event.getKeySym()=="Left")
            {
                mtx_.lock();
                current_iteration_--;
                processed_=false;
                mtx_.unlock();
            }

            if(event.getKeySym()=="s")
            {
                if(show_samples_)
                    show_samples_=false;
                else
                    show_samples_=true;

                if(show_samples_)
                    cout<<"show samples"<<endl;
                else
                    cout<<"not show samples"<<endl;
            }
        }
        //draw.unlock();
    //}
}

void BatchProcessing::startPlotTrialResults()
{
    interactive_ = widget.interactivePlotTrial->isChecked();
    if(interactive_)
        current_iteration_ = widget.startPosePlotTrial->value();
    else
        current_iteration_ = 0;
    
    trial_folder =  trial_folder / widget.plotTrialPath->text().toStdString();
            
    //Start visualization thread
    visualization_process = new boost::thread(boost::bind(&BatchProcessing::visualization,this)); 
    //Start tests thread
    generic_process = new boost::thread(boost::bind(&BatchProcessing::trialPloter,this)); 
}

void BatchProcessing::trialPloter()
{
    boost::filesystem::path pcd_filtered_path(trial_folder / "pcd_filtered" / "data");
    boost::filesystem::path pose_text_folder(trial_folder / "pose_text" / "data");
    boost::filesystem::path pose_XML_folder(trial_folder / "pose_xml" / "data");
    
    //Get iterators
    boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
    boost::filesystem::directory_iterator itr(pcd_filtered_path);
    
    std::vector<boost::filesystem::path> paths;
    
    //Count the number of files
    for(;itr != end_itr;itr++)
    {
        if(boost::filesystem::is_regular_file(itr->status()))
            paths.push_back(itr->path());
    }
    
    //Sort files
    std::sort(paths.begin(),paths.end());
    
    if(paths.size()==0)
    {
        cerr<<"No files to process in: "<<trial_folder.string()<<endl;
        return;
    }
    
    //Pose estimation class
    PoseEstimation pose_estimation;
    
    int count = 0;
    
    while(!stop_)
    {
        mtx_.lock();
        if(!processed_)
        {
            current_iteration_ = max(current_iteration_,(uint)0);
            current_iteration_ = min(current_iteration_,(uint)(paths.size()-1));

            //Load non filtered point cloud
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
            
            current_file_name = (paths[current_iteration_].filename()).string();
            
            if(count>0)
            {
                draw.lock();
                viewer->removeShape("status");
                viewer->addText((boost::format("Loading: %1% ...")%paths[current_iteration_].string()).str(),5,20,16,1.0,0.3,0.,"status");
                draw.unlock();
            }
            
            cout<<"loading: "<<paths[current_iteration_].string()<<endl;
            if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (paths[current_iteration_].string(), *cloud) == -1) //* load the file
            {
                PCL_ERROR ("Couldn't read pcd file \n");
                break;
            }
            
            draw.lock();
            viewer->removeShape("status");
            viewer->addText((boost::format("Processing: %1% ...")%paths[current_iteration_].string()).str(),5,20,16,1.0,0.3,0.,"status");
            draw.unlock();
            
            cout<<"processing: "<<paths[current_iteration_].string()<<endl;
            pose_estimation.segmentPedestrian(cloud);
            cout<<"getting pose"<<endl;
            
            string pose_text_file = (pose_text_folder/current_file_name).string();
            pose_text_file.replace(pose_text_file.find("."),string::npos,".txt");
            
            pose_estimation.loadPoseText(pose_text_file);
            
            /*string pose_XML_file = (pose_XML_folder/current_file_name).string();
            pose_XML_file.replace(pose_XML_file.find("."),string::npos,".xml");
            
            pose_estimation.loadPoseXML(pose_XML_file);*/
            
            draw.lock();
            c_position_ = pose_estimation.model_.head.start_ - makePoint(-1,0,3);
            c_look_at_ = pose_estimation.model_.center_torso.start_;
            c_orientation_ = makePoint(0,1,0);
            
            viewer->removeShape("status");
            viewer->addText(string("Drawing pose ..."),5,20,16,0.,0.,1.,"status");
            draw.unlock();
            
            cout<<"drawing pose"<<endl;
            drawSimplePose(pose_estimation,pose_estimation.model_);
            
            draw.lock();
            viewer->removeShape("status");
            viewer->addText((boost::format("Ready: %1%")%paths[current_iteration_].string()).str(),5,20,16,0.,1.,0.,"status");
            draw.unlock();
            
            cout<<"done"<<endl;
            processed_ = true;
            count++;
        }
        mtx_.unlock();
        
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
        
        if(!interactive_)
        {
            current_iteration_++;
            processed_ = false;
            
            //Finish with this trial
            if(current_iteration_>=paths.size())
                return;
        }
    }
}

void BatchProcessing::visualization()
{
    viewer.reset(new pcl::visualization::PCLVisualizer);
    
    //Create visualization
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(buffer);
    viewer->addPointCloud<pcl::PointXYZRGB> (buffer, rgb, "pedestrian");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "pedestrian");
    
    //Viewer properties
    viewer->setBackgroundColor (0.6, 0.6, 0.6);
    viewer->addCoordinateSystem (1.0);
    
    //Register a keyboard handler function
    viewer->registerKeyboardCallback(boost::bind(&BatchProcessing::keyboardHandler,this,_1));
    viewer->setWindowName("Pose_Estimation");
    //viewer->setPosition (50,50);
    //viewer->setSize (500,800);//width, height
    //viewer->setFullScreen(true);
    
    viewer->initCameraParameters ();
    
    while (!viewer->wasStopped ())
    {
        draw.lock();
        viewer->spinOnce (100);
        if(new_pose_ && save_screenshot_)
        {
            //viewer->saveScreenshot("/home/jorge/Escritorio/test.png");
            new_pose_=false;
    
            boost::filesystem::path screen_shots_folder(trial_folder / "screen_shots" / "data");
            
            //Create the disparity folder
            boost::filesystem::create_directories(screen_shots_folder);
    
            string screen_shot_file = (screen_shots_folder/current_file_name).string();
            screen_shot_file.replace(screen_shot_file.find("."),string::npos,".png");
                    
            string cmd = (boost::format("import -window Pose_Estimation %1%")%screen_shot_file).str();
            system(cmd.c_str());
            cout<<"saved screenshot to: "<<screen_shot_file<<endl;
        }
        
        draw.unlock();
        
        boost::this_thread::sleep (boost::posix_time::microseconds (10000));
    }
    
    stop_ = true;
    
    viewer->close();
}