#include "poseEstimation.h"

using namespace std;
using namespace rapidxml;

void print(ostream& o,const pcl::PointXYZRGB& p)
{
    o<<std::fixed<<std::setprecision(5)<<p.x<<" "<<p.y<<" "<<p.z<<endl;
    return;
}

void PoseEstimation::writeVersion(string file)
{
    std::ofstream out_file(file.c_str(),std::ios_base::out);
    out_file<<"Stereo Pedestrian Pose Estimator"<<endl<<"version: 0.1"<<endl<<"Jorge Almeida"<<endl;
    out_file<<endl;
    out_file<<"Data file format:"<<endl<<endl;
    out_file<<"head.x head.y head.z"<<endl;
    out_file<<"neck.x neck.y neck.z"<<endl;
    out_file<<"shoulders_left.x shoulders_left.y shoulders_left.z"<<endl;
    out_file<<"shoulders_right.x shoulders_right.y shoulders_right.z"<<endl;
    out_file<<"center_torso.x center_torso.y center_torso.z"<<endl;
    out_file<<"center_torso_left.x center_torso_left.y center_torso_left.z"<<endl;
    out_file<<"center_torso_right.x center_torso_right.y center_torso_right.z"<<endl;
    out_file<<"waist.x waist.y waist.z"<<endl;
    out_file<<"hips_left.x hips_left.y hips_left.z"<<endl;
    out_file<<"hips_right.x hips_right.y hips_right.z"<<endl;
    out_file<<"left_knee.x left_knee.y left_knee.z"<<endl;
    out_file<<"right_knee.x right_knee.y right_knee.z"<<endl;
    out_file<<"left_foot.x left_foot.y left_foot.z"<<endl;
    out_file<<"right_foot.x right_foot.y right_foot.z"<<endl;
            
    out_file.close();
}

void PoseEstimation::loadPoseText(std::string file_name)
{
    std::ifstream in_file(file_name.c_str(),std::ifstream::in);

    if(!in_file.good())
    {
        cerr<<"Error opening "<<file_name<<endl;
        cerr<<"Unable to load pose data."<<endl;
        return;
    }

    in_file>>model_.head.start_.x;
    in_file>>model_.head.start_.y;
    in_file>>model_.head.start_.z;
    
    model_.head.end_ = model_.head.start_;
    model_.neck.start_ = model_.head.end_;
            
    in_file>>model_.neck.end_.x;
    in_file>>model_.neck.end_.y;
    in_file>>model_.neck.end_.z;
    
    in_file>>model_.shoulders.left_.x;
    in_file>>model_.shoulders.left_.y;
    in_file>>model_.shoulders.left_.z;
    
    in_file>>model_.shoulders.right_.x;
    in_file>>model_.shoulders.right_.y;
    in_file>>model_.shoulders.right_.z;
    
    model_.shoulders.start_ = model_.neck.end_;
    model_.shoulders.end_ = model_.neck.end_;
    
    in_file>>model_.center_torso.start_.x;
    in_file>>model_.center_torso.start_.y;
    in_file>>model_.center_torso.start_.z;
    
    model_.center_torso.end_ = model_.center_torso.start_;
    
    model_.center_torso.detected_ = true;
            
    in_file>>model_.center_torso.left_.x;
    in_file>>model_.center_torso.left_.y;
    in_file>>model_.center_torso.left_.z;
    
    in_file>>model_.center_torso.right_.x;
    in_file>>model_.center_torso.right_.y;
    in_file>>model_.center_torso.right_.z;
    
    in_file>>model_.torso.end_.x;
    in_file>>model_.torso.end_.y;
    in_file>>model_.torso.end_.z;
    
    model_.torso.start_ = model_.center_torso.end_;
    
    in_file>>model_.hips.left_.x;
    in_file>>model_.hips.left_.y;
    in_file>>model_.hips.left_.z;
    
    in_file>>model_.hips.right_.x;
    in_file>>model_.hips.right_.y;
    in_file>>model_.hips.right_.z;
    
    model_.hips.start_ = model_.torso.end_;
    model_.hips.end_ = model_.torso.end_;
    
    model_.hips.detected_ = true;
            
    in_file>>model_.left_leg.end_.x;
    in_file>>model_.left_leg.end_.y;
    in_file>>model_.left_leg.end_.z;
    
    model_.left_leg.start_ = model_.hips.left_;
            
    in_file>>model_.right_leg.end_.x;
    in_file>>model_.right_leg.end_.y;
    in_file>>model_.right_leg.end_.z;
    
    model_.right_leg.start_ = model_.hips.right_;
    
    in_file>>model_.left_foot.end_.x;
    in_file>>model_.left_foot.end_.y;
    in_file>>model_.left_foot.end_.z;
    
    model_.left_foot.start_ = model_.left_leg.end_;
            
    in_file>>model_.right_foot.end_.x;
    in_file>>model_.right_foot.end_.y;
    in_file>>model_.right_foot.end_.z;
    
    model_.right_foot.start_ = model_.right_leg.end_;
    
    in_file.close();
    
    return;
}

void PoseEstimation::savePoseText(std::string file_name)
{
    std::ofstream out_file(file_name.c_str(),std::ios_base::out);

    if(!out_file.is_open())
    {
        cerr<<"Error opening "<<file_name<<endl;
        cerr<<"Unable to save pose data."<<endl;
        return;
    }
    
    print(out_file,model_.head.start_);
    print(out_file,model_.neck.end_);
    print(out_file,model_.shoulders.left_);
    print(out_file,model_.shoulders.right_);
    print(out_file,model_.center_torso.start_);
    print(out_file,model_.center_torso.left_);
    print(out_file,model_.center_torso.right_);
    print(out_file,model_.torso.end_);
    print(out_file,model_.hips.left_);
    print(out_file,model_.hips.right_);
    print(out_file,model_.left_leg.end_);
    print(out_file,model_.right_leg.end_);
    print(out_file,model_.left_foot.end_);
    print(out_file,model_.right_foot.end_);
    
    out_file.close();
    
    return;
}

void PoseEstimation::savePoseXML(std::string file_name)
{
    xml_node<>* decl = doc.allocate_node(node_declaration);
    decl->append_attribute(doc.allocate_attribute("version", "1.0"));
    decl->append_attribute(doc.allocate_attribute("encoding", "utf-8"));
    doc.append_node(decl);

    xml_node<>* root = doc.allocate_node(node_element, "pose");
    root->append_attribute(doc.allocate_attribute("version", "0.1"));
    doc.append_node(root);

    xml_node<>* child = doc.allocate_node(node_element, "trial");
    child->value("unset");
    root->append_node(child);
    
    child = doc.allocate_node(node_element, "cloud_file");
    child->value("unset");
    root->append_node(child);

    child = doc.allocate_node(node_element, "height");
    
    std::string str = boost::lexical_cast<string>(model_.height_);
    child->value(doc.allocate_string(str.c_str()));
    root->append_node(child);
    
    xml_node<>* ratios = doc.allocate_node(node_element, "body_segment_to_height_ratios");
    root->append_node(ratios);

    //Add all body segment ratios
    std::map<std::string,double>::iterator it;
    for(it=model_.body_segment_to_height_ratio_.begin();it!=model_.body_segment_to_height_ratio_.end();it++)
    {
        child = doc.allocate_node(node_element, "segment");
        child->append_attribute(doc.allocate_attribute("name",it->first.c_str()));
        
        std::string str = boost::lexical_cast<string>(it->second);
        child->append_attribute(doc.allocate_attribute("value",doc.allocate_string(str.c_str())));
        ratios->append_node(child);
    }
    
    //Add all body parts
    xml_node<>* body_parts = doc.allocate_node(node_element, "body_parts");
    root->append_node(body_parts);
    
    writeBodyPartXML(body_parts,model_.head,"head");
    writeBodyPartXML(body_parts,model_.neck,"neck");
    writeBodyPartXML(body_parts,model_.shoulders,"shoulders");
    writeBodyPartXML(body_parts,model_.upper_torso,"upper_torso");
    writeBodyPartXML(body_parts,model_.torso,"torso");
    writeBodyPartXML(body_parts,model_.center_torso,"center_torso");
    writeBodyPartXML(body_parts,model_.hips,"hips");
    writeBodyPartXML(body_parts,model_.left_leg,"left_leg");
    writeBodyPartXML(body_parts,model_.right_leg,"right_leg");
    writeBodyPartXML(body_parts,model_.left_foot,"left_foot");
    writeBodyPartXML(body_parts,model_.right_foot,"right_foot");
    
    // Convert doc to string if needed
    std::string xml_as_string;
    rapidxml::print(std::back_inserter(xml_as_string), doc);

    // Save to file
    std::ofstream file_stored(file_name.c_str());
    file_stored << doc;
    file_stored.close();
    doc.clear();
}

void PoseEstimation::writeBodyPartXML(xml_node<>* parent_node,BodyPart& part,string name)
{
    xml_node<>* node = doc.allocate_node(node_element, "body_part");
    node->append_attribute(doc.allocate_attribute("name",doc.allocate_string(name.c_str())));
    
    parent_node->append_node(node);
    
    xml_node<>* child = doc.allocate_node(node_element, "detected");
    
    if(part.detected_)
        child->value("true");
    else
        child->value("false");
    
    node->append_node(child);
    
    child = doc.allocate_node(node_element, "cloud");
    child->value("unset");
    node->append_node(child);
    
    writePointXYZRGBXML(node,part.start_,"start");
    writePointXYZRGBXML(node,part.end_,"end");
    writePointXYZRGBXML(node,part.left_,"left");
    writePointXYZRGBXML(node,part.right_,"right");
    writePointXYZRGBXML(node,part.preferencial_direction_,"preferencial_direction");
    writePointXYZRGBXML(node,part.preferencial_end_,"preferencial_end");
    
    if(part.ellipse_!=0)
        writeEllipseXML(node,*(part.ellipse_),"ellipse");
}

void PoseEstimation::writePointXYZRGBXML(xml_node<>* parent_node,pcl::PointXYZRGB point,string name)
{
    xml_node<>* node = doc.allocate_node(node_element,doc.allocate_string(name.c_str()));
    
    std::string x_val = boost::lexical_cast<string>(point.x);
    std::string y_val = boost::lexical_cast<string>(point.y);
    std::string z_val = boost::lexical_cast<string>(point.z);
    
    node->append_attribute(doc.allocate_attribute("x",doc.allocate_string(x_val.c_str())));
    node->append_attribute(doc.allocate_attribute("y",doc.allocate_string(y_val.c_str())));
    node->append_attribute(doc.allocate_attribute("z",doc.allocate_string(z_val.c_str())));
    
    parent_node->append_node(node);
}

void PoseEstimation::writeEllipseXML(xml_node<>* parent_node,Ellipse& ellipse,string name)
{
    xml_node<>* node = doc.allocate_node(node_element,doc.allocate_string(name.c_str()));
    
    xml_node<>* a_node = doc.allocate_node(node_element,"a");
    std::string a_val = boost::lexical_cast<string>(ellipse.a_);
    a_node->value(doc.allocate_string(a_val.c_str()));
    node->append_node(a_node);
    
    xml_node<>* b_node = doc.allocate_node(node_element,"b");
    std::string b_val = boost::lexical_cast<string>(ellipse.b_);
    b_node->value(doc.allocate_string(b_val.c_str()));
    node->append_node(b_node);
    
    xml_node<>* theta_node = doc.allocate_node(node_element,"theta");
    std::string theta_val = boost::lexical_cast<string>(ellipse.theta_);
    theta_node->value(doc.allocate_string(theta_val.c_str()));
    node->append_node(theta_node);
    
    xml_node<>* phi_node = doc.allocate_node(node_element,"phi");
    std::string phi_val = boost::lexical_cast<string>(ellipse.phi_);
    phi_node->value(doc.allocate_string(phi_val.c_str()));
    node->append_node(phi_node);
    
    writePointXYZRGBXML(node,ellipse.center_,"center");
    writePointXYZRGBXML(node,makePoint(ellipse.u_),"u");
    writePointXYZRGBXML(node,makePoint(ellipse.v_),"v");
            
    parent_node->append_node(node);
}

void PoseEstimation::loadPoseXML(std::string file_name)
{
    // Read the xml file into a vector
    ifstream data(file_name.c_str());
    
    if(!data.good())
    {
        cerr<<"Error opening "<<file_name<<endl;
        cerr<<"Unable to load pose data."<<endl;
        return;
    }
        
    vector<char> buffer((istreambuf_iterator<char>(data)), 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
    xml_node<>* root_node = doc.first_node("pose");
    
    model_.height_ = atof(root_node->first_node("height")->value());
    
    xml_node<>*ratios=root_node->first_node("body_segment_to_height_ratios");
    
    //Get body segment ratios
    for(xml_node<>*ratio = ratios->first_node("segment");ratio;ratio = ratio->next_sibling())
    {
        string name = ratio->first_attribute("name")->value();
        double value = atof(ratio->first_attribute("value")->value());
        
        model_.body_segment_to_height_ratio_[name]=value;
    }
    
    //Load body parts
    xml_node<>*parts=root_node->first_node("body_parts");
    
    for(xml_node<>*part=parts->first_node("body_part");part;part = part->next_sibling())
    {
        string name = part->first_attribute("name")->value();
        
        if(name == "head")
            loadBodyPartXML(part,model_.head);
        
        if(name == "neck")
            loadBodyPartXML(part,model_.neck);
        
        if(name == "shoulders")
            loadBodyPartXML(part,model_.shoulders);
        
        if(name == "upper_torso")
            loadBodyPartXML(part,model_.upper_torso);
        
        if(name == "torso")
            loadBodyPartXML(part,model_.torso);
        
        if(name == "center_torso")
            loadBodyPartXML(part,model_.center_torso);
        
        if(name == "hips")
            loadBodyPartXML(part,model_.hips);
        
        if(name == "left_leg")
            loadBodyPartXML(part,model_.left_leg);
        
        if(name == "right_leg")
            loadBodyPartXML(part,model_.right_leg);
        
        if(name == "left_foot")
            loadBodyPartXML(part,model_.left_foot);
        
        if(name == "right_foot")
            loadBodyPartXML(part,model_.right_foot);
    }
}

void PoseEstimation::loadBodyPartXML(xml_node<>*node,BodyPart& part)
{
    xml_node<>*detected_node = node->first_node("detected");
    
    string det = detected_node->value();
    
    if(det=="true")
        part.detected_ = true;
    else
        part.detected_ = false;
    
    loadPointXYZRGBXMP(node->first_node("start"),part.start_);
    loadPointXYZRGBXMP(node->first_node("end"),part.end_);
    loadPointXYZRGBXMP(node->first_node("left"),part.left_);
    loadPointXYZRGBXMP(node->first_node("right"),part.right_);
    loadPointXYZRGBXMP(node->first_node("preferencial_direction"),part.preferencial_direction_);
    loadPointXYZRGBXMP(node->first_node("preferencial_end"),part.preferencial_end_);
}

void PoseEstimation::loadPointXYZRGBXMP(xml_node<>*node,pcl::PointXYZRGB& point)
{
    point.x=atof(node->first_attribute("x")->value());
    point.y=atof(node->first_attribute("y")->value());
    point.z=atof(node->first_attribute("z")->value());
}
        
void PoseEstimation::segmentPedestrian(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_original)
{
    //Remove invalid points, losing organization
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    vector<double> limits = boost::assign::list_of<double>(-10)(10)(-10)(10)(0)(20);
    conditionalFilter(cloud_original,cloud,limits);
    
    cloud_pedestrian_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

    //Do euclidean clustering and extract the largest cluster
    //Set kdtree input cloud
    tree_->setInputCloud (cloud);

    vector<pcl::PointIndices> cluster_indices;

    euclidean_clustering_.setClusterTolerance (0.1);
    euclidean_clustering_.setMinClusterSize (100);
    euclidean_clustering_.setMaxClusterSize (25000);
    euclidean_clustering_.setSearchMethod(tree_);
    euclidean_clustering_.setInputCloud(cloud);
    euclidean_clustering_.extract (cluster_indices);

    //Get the largest cluster
    if(cluster_indices.size()==0)
    {
        cerr<<"Unable to perform euclidean pedestrian extraction"<<endl;
        pcl::copyPointCloud (*cloud,*cloud_pedestrian_);
        return;
    }
        
    int inx = distance(cluster_indices.begin(),max_element(cluster_indices.begin(),cluster_indices.end(),compareIndices));

    // Extract the inliers to cloud_pedestrian_
    extract_indices_.setInputCloud (cloud);
    extract_indices_.setIndices (boost::make_shared<pcl::PointIndices>(cluster_indices[inx]));
    extract_indices_.setNegative (false);
    extract_indices_.filter(*cloud_pedestrian_);
}

void PoseEstimation::upperLegComposedSegmentation()
{
    Colormap colormap("hsv_small");
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_working(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_auxiliar(new pcl::PointCloud<pcl::PointXYZRGB>);
    
    cout<<"locate left knee"<<endl;
    //model_.left_leg.sampling_.reset(new EllipseSampling);
    model_.left_leg.sampling_.reset(new CosineBiasedSampling);
    model_.left_leg.sampling_->start_point_ = model_.hips.left_;
    model_.left_leg.sampling_->number_samples_ = 100;
    model_.left_leg.start_ = model_.hips.left_;
    model_.left_leg.sampling_->preferencial_end_ = model_.hips.left_;
    model_.left_leg.sampling_->preferencial_end_.y+=-model_.getSegmentLength("upper_leg");
    
    //model_.left_leg.sampling_->preferencial_direction_ = makePoint(0,0,-1);
    model_.left_leg.sampling_->preferencial_direction_ = model_.main_direction;
    
    model_.left_leg.sampling_->max_theta_=pcl::deg2rad(60.0);
    //model_.left_leg.sampling_->max_phi_=pcl::deg2rad(10.0);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->positive_max_angle_=pcl::deg2rad(-40.);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->negative_max_angle_=pcl::deg2rad(-30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->negative_deviation_=pcl::deg2rad(10.);
    
    model_.left_leg.sampling_->createSamples();
    model_.left_leg.sampling_->scoreSamples(cloud_bellow_waist_);
    
    int max_indx_left_knee = model_.left_leg.sampling_->getBestSampleIndex();
    double max_score_left_knee = model_.left_leg.sampling_->scores_[max_indx_left_knee];
    pcl::PointXYZRGB p_left_knee_trial = reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->samples_[max_indx_left_knee];
    
    cout<<"max left knee located, creating segmented cloud"<<endl;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_waist_left_knee_removed(new pcl::PointCloud<pcl::PointXYZRGB>);
    removeExplainedPoints(model_.left_leg.sampling_->distances_,max_indx_left_knee,0.15,cloud_bellow_waist_,cloud_bellow_waist_left_knee_removed,cloud_auxiliar);
    
    cout<<"locate right knee"<<endl;
    //model_.right_leg.sampling_.reset(new EllipseSampling);
    model_.right_leg.sampling_.reset(new CosineBiasedSampling);
    model_.right_leg.sampling_->start_point_ = model_.hips.right_;
    model_.right_leg.sampling_->number_samples_ = 100;
    model_.right_leg.start_ = model_.hips.right_;
    model_.right_leg.sampling_->preferencial_end_ = model_.hips.right_;
    model_.right_leg.sampling_->preferencial_end_.y+=-model_.getSegmentLength("upper_leg");
    //model_.right_leg.sampling_->preferencial_direction_ = makePoint(0,0,-1);
    model_.right_leg.sampling_->preferencial_direction_ = model_.main_direction;
    model_.right_leg.sampling_->max_theta_=pcl::deg2rad(60.0);
    //model_.right_leg.sampling_->max_phi_=pcl::deg2rad(10.0);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->positive_max_angle_=pcl::deg2rad(40.);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->negative_max_angle_=pcl::deg2rad(30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->negative_deviation_=pcl::deg2rad(-10.);
    
    model_.right_leg.sampling_->createSamples();
    model_.right_leg.sampling_->scoreSamples(cloud_bellow_waist_);
    
    int max_indx_right_knee = model_.right_leg.sampling_->getBestSampleIndex();
    double max_score_right_knee = model_.right_leg.sampling_->scores_[max_indx_right_knee];
    pcl::PointXYZRGB p_right_knee_trial = reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->samples_[max_indx_right_knee];
    
    cout<<"max right knee located, creating segmented cloud"<<endl;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_waist_right_knee_removed(new pcl::PointCloud<pcl::PointXYZRGB>);
    removeExplainedPoints(model_.right_leg.sampling_->distances_,max_indx_right_knee,0.15,cloud_bellow_waist_,cloud_bellow_waist_right_knee_removed,cloud_auxiliar);

    //Now check witch one is best

    cout<<"left score: "<<max_score_left_knee<<endl;
    cout<<"right score: "<<max_score_right_knee<<endl;

    //If left leg is better, remove points and detect the right leg
    if(max_score_left_knee>max_score_right_knee)
    {
        cout<<"left knee is better"<<endl;
        
        excludeSamplesByProximityToLineSegment(model_.left_leg.start_,p_left_knee_trial,reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->samples_,0.15);
        model_.right_leg.sampling_->scoreSamples(cloud_bellow_waist_left_knee_removed);
        max_indx_right_knee = model_.right_leg.sampling_->getBestSampleIndex();
        
        model_.right_leg.end_ = reinterpret_cast<CosineBiasedSampling*>(model_.right_leg.sampling_.get())->samples_[max_indx_right_knee];
        model_.left_leg.end_ = p_left_knee_trial;

        removeExplainedPointsExcludingPointNeighborhood(model_.left_leg.end_,model_.left_leg.sampling_->distances_,max_indx_left_knee,0.15,cloud_bellow_waist_,cloud_bellow_waist_left_knee_removed,model_.left_leg.cloud_);
        colormap.setColor(model_.left_leg.cloud_,PedestrianModel::LEFT_KNEE);
        *cloud_accumulation_ += *model_.left_leg.cloud_;

        //The point cloud cloud_bellow_waist_left_knee_removed, changed from the previous step, i need to correct the distances matrix (or recalculate it)
        model_.right_leg.sampling_->scoreSamples(cloud_bellow_waist_left_knee_removed);
        
        removeExplainedPointsExcludingPointNeighborhood(model_.right_leg.end_,model_.right_leg.sampling_->distances_,max_indx_right_knee,0.15,cloud_bellow_waist_left_knee_removed,cloud_bellow_knees_,model_.right_leg.cloud_);
        colormap.setColor(model_.right_leg.cloud_,PedestrianModel::RIGHT_KNEE);
        *cloud_accumulation_ += *model_.right_leg.cloud_;
        
    }else//right knee fits best
    {
        cout<<"right knee is better"<<endl;

        excludeSamplesByProximityToLineSegment(model_.right_leg.start_,p_right_knee_trial,reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->samples_,0.15);
        model_.left_leg.sampling_->scoreSamples(cloud_bellow_waist_right_knee_removed);
        max_indx_left_knee = model_.left_leg.sampling_->getBestSampleIndex();

        model_.left_leg.end_ = reinterpret_cast<CosineBiasedSampling*>(model_.left_leg.sampling_.get())->samples_[max_indx_left_knee];
        model_.right_leg.end_ = p_right_knee_trial;
        
        cloud_bellow_waist_right_knee_removed->clear();
        removeExplainedPointsExcludingPointNeighborhood(model_.right_leg.end_,model_.right_leg.sampling_->distances_,max_indx_right_knee,0.15,cloud_bellow_waist_,cloud_bellow_waist_right_knee_removed,model_.right_leg.cloud_);
        colormap.setColor(model_.right_leg.cloud_,PedestrianModel::RIGHT_KNEE);
        *cloud_accumulation_ += *model_.right_leg.cloud_;

        //The point cloud cloud_bellow_waist_left_knee_removed, changed from the previous step, i need to correct the distances matrix (or recalculate it)
        model_.left_leg.sampling_->scoreSamples(cloud_bellow_waist_right_knee_removed);
        
        removeExplainedPointsExcludingPointNeighborhood(model_.left_leg.end_,model_.left_leg.sampling_->distances_,max_indx_left_knee,0.15,cloud_bellow_waist_right_knee_removed,cloud_bellow_knees_,model_.left_leg.cloud_);
        colormap.setColor(model_.left_leg.cloud_,PedestrianModel::LEFT_KNEE);
        *cloud_accumulation_ += *model_.left_leg.cloud_;
    }
    
    model_.left_leg.detected_ = true;
    model_.right_leg.detected_ = true;
}

void PoseEstimation::upperArmIndependentSegmentation()
{
    Colormap colormap("hsv_small");
    
    cout<<"locate left upper arm"<<endl;
    model_.left_upper_arm.sampling_.reset(new CosineBiasedSamplingModified);
    model_.left_upper_arm.sampling_->start_point_ = model_.shoulders.left_;
    model_.left_upper_arm.sampling_->number_samples_ = 50;
    model_.left_upper_arm.start_ = model_.left_upper_arm.sampling_->start_point_;
    model_.left_upper_arm.sampling_->preferencial_end_ = model_.left_upper_arm.start_;
    model_.left_upper_arm.sampling_->preferencial_end_.y+=-model_.getSegmentLength("upper_arm");
    model_.left_upper_arm.sampling_->preferencial_direction_ = model_.main_direction;
    
    model_.left_upper_arm.sampling_->max_theta_=pcl::deg2rad(60.0);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_upper_arm.sampling_.get())->positive_max_angle_=pcl::deg2rad(-30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_upper_arm.sampling_.get())->negative_max_angle_=pcl::deg2rad(-30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.left_upper_arm.sampling_.get())->negative_deviation_=pcl::deg2rad(0.);
    
    model_.left_upper_arm.sampling_->createSamples();
    model_.left_upper_arm.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_left_upper_arm = model_.left_upper_arm.sampling_->getBestSampleIndex();
    //double max_score_left_upper_arm = model_.left_upper_arm.sampling_->scores_[max_indx_left_upper_arm];
    model_.left_upper_arm.end_ = reinterpret_cast<CosineBiasedSampling*>(model_.left_upper_arm.sampling_.get())->samples_[max_indx_left_upper_arm];
    
    removeExplainedPoints(model_.left_upper_arm.sampling_->distances_,max_indx_left_upper_arm,0.15,cloud_top_body_,cloud_top_body_,model_.left_upper_arm.cloud_);
    colormap.setColor(model_.left_upper_arm.cloud_,PedestrianModel::LEFT_UPPER_ARM);
    *cloud_accumulation_ += *model_.left_upper_arm.cloud_;
    
    cout<<"upper left arm located"<<endl;
    
    cout<<"locate right upper arm"<<endl;
    model_.right_upper_arm.sampling_.reset(new CosineBiasedSamplingModified);
    model_.right_upper_arm.sampling_->start_point_ = model_.shoulders.right_;
    model_.right_upper_arm.sampling_->number_samples_ = 50;
    model_.right_upper_arm.start_ = model_.right_upper_arm.sampling_->start_point_;
    model_.right_upper_arm.sampling_->preferencial_end_ = model_.right_upper_arm.start_;
    model_.right_upper_arm.sampling_->preferencial_end_.y+=-model_.getSegmentLength("upper_arm");
    model_.right_upper_arm.sampling_->preferencial_direction_ = model_.main_direction;
    
    model_.right_upper_arm.sampling_->max_theta_=pcl::deg2rad(60.0);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_upper_arm.sampling_.get())->positive_max_angle_=pcl::deg2rad(30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_upper_arm.sampling_.get())->negative_max_angle_=pcl::deg2rad(30.);
    reinterpret_cast<CosineBiasedSampling*>(model_.right_upper_arm.sampling_.get())->negative_deviation_=pcl::deg2rad(0.);
    
    model_.right_upper_arm.sampling_->createSamples();
    model_.right_upper_arm.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_right_upper_arm = model_.right_upper_arm.sampling_->getBestSampleIndex();
    model_.right_upper_arm.end_ = reinterpret_cast<CosineBiasedSampling*>(model_.right_upper_arm.sampling_.get())->samples_[max_indx_right_upper_arm];
    
    removeExplainedPoints(model_.right_upper_arm.sampling_->distances_,max_indx_right_upper_arm,0.15,cloud_top_body_,cloud_top_body_,model_.right_upper_arm.cloud_);
    colormap.setColor(model_.right_upper_arm.cloud_,PedestrianModel::RIGHT_UPPER_ARM);
    *cloud_accumulation_ += *model_.right_upper_arm.cloud_;
 
    model_.left_upper_arm.detected_ = true;
    model_.right_upper_arm.detected_ = true;
    
    cout<<"upper right arm located"<<endl;
}

void PoseEstimation::lowerArmIndependentSegmentation()
{
    Colormap colormap("hsv_small");
    
    cout<<"locate left lower arm"<<endl;
    model_.left_lower_arm.sampling_.reset(new OffCenterEllipseSamplingModified);
    model_.left_lower_arm.sampling_->start_point_ = model_.left_upper_arm.end_;
    model_.left_lower_arm.sampling_->number_samples_ = 50;
    model_.left_lower_arm.start_ = model_.left_lower_arm.sampling_->start_point_;
    model_.left_lower_arm.sampling_->preferencial_end_ = model_.left_upper_arm.end_;
    
    model_.left_lower_arm.sampling_->preferencial_end_ =  model_.left_upper_arm.end_ + model_.getSegmentLength("lower_arm")*normalize(model_.left_upper_arm.end_-model_.left_upper_arm.start_);
    model_.left_lower_arm.sampling_->preferencial_direction_ = model_.main_direction;
    
    model_.left_lower_arm.sampling_->max_theta_=pcl::deg2rad(60.0);
    model_.left_lower_arm.sampling_->max_phi_=pcl::deg2rad(20.0);
    reinterpret_cast<OffCenterEllipseSampling*>(model_.left_lower_arm.sampling_.get())->off_center_ = pcl::deg2rad(45.0);
    
    model_.left_lower_arm.sampling_->createSamples();
    model_.left_lower_arm.sampling_->scoreSamples(cloud_pedestrian_);
    
    int max_indx_left_lower_arm = model_.left_lower_arm.sampling_->getBestSampleIndex();
    model_.left_lower_arm.end_ = reinterpret_cast<OffCenterEllipseSamplingModified*>(model_.left_lower_arm.sampling_.get())->samples_[max_indx_left_lower_arm];
    
    removeExplainedPoints(model_.left_lower_arm.sampling_->distances_,max_indx_left_lower_arm,0.12,cloud_pedestrian_,cloud_removed_arms_,model_.left_lower_arm.cloud_);
    colormap.setColor(model_.left_lower_arm.cloud_,PedestrianModel::LEFT_LOWER_ARM);
    *cloud_accumulation_ += *model_.left_lower_arm.cloud_;
    
    cout<<"lower left arm located"<<endl;
    
    cout<<"locate right lower arm"<<endl;
    model_.right_lower_arm.sampling_.reset(new OffCenterEllipseSamplingModified);
    model_.right_lower_arm.sampling_->start_point_ = model_.right_upper_arm.end_;
    model_.right_lower_arm.sampling_->number_samples_ = 50;
    model_.right_lower_arm.start_ = model_.right_lower_arm.sampling_->start_point_;
    model_.right_lower_arm.sampling_->preferencial_end_ = model_.right_upper_arm.end_;
    
    model_.right_lower_arm.sampling_->preferencial_end_ =  model_.right_upper_arm.end_ + model_.getSegmentLength("lower_arm")*normalize(model_.right_upper_arm.end_-model_.right_upper_arm.start_);
    model_.right_lower_arm.sampling_->preferencial_direction_ = model_.main_direction;
    
    model_.right_lower_arm.sampling_->max_theta_=pcl::deg2rad(60.0);
    model_.right_lower_arm.sampling_->max_phi_=pcl::deg2rad(20.0);
    reinterpret_cast<OffCenterEllipseSampling*>(model_.right_lower_arm.sampling_.get())->off_center_ = pcl::deg2rad(45.0);
    
    model_.right_lower_arm.sampling_->createSamples();
    model_.right_lower_arm.sampling_->scoreSamples(cloud_removed_arms_);
    
    int max_indx_right_lower_arm = model_.right_lower_arm.sampling_->getBestSampleIndex();
    model_.right_lower_arm.end_ = reinterpret_cast<OffCenterEllipseSamplingModified*>(model_.right_lower_arm.sampling_.get())->samples_[max_indx_right_lower_arm];
    
    removeExplainedPoints(model_.right_lower_arm.sampling_->distances_,max_indx_right_lower_arm,0.12,cloud_removed_arms_,cloud_removed_arms_,model_.right_lower_arm.cloud_);
    colormap.setColor(model_.right_lower_arm.cloud_,PedestrianModel::RIGHT_LOWER_ARM);
    *cloud_accumulation_ += *model_.right_lower_arm.cloud_;
    
    model_.left_lower_arm.detected_ = true;
    model_.right_lower_arm.detected_ = true;
    
    cout<<"lower right arm located"<<endl;
}

void PoseEstimation::getPose()
{
    Colormap colormap("hsv_small");
    
    //Get head position
    //Eigen::Vector4f centroid;
    //pcl::compute3DCentroid<pcl::PointXYZRGB>(*cloud_filtered,centroid);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_working(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_auxiliar(new pcl::PointCloud<pcl::PointXYZRGB>);
    
    cloud_accumulation_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_bellow_knees_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_top_body_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_bellow_waist_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_removed_arms_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_knees_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    
    Eigen::Vector4f min_pt;
    Eigen::Vector4f max_pt;
    
    cout<<"pedestrian cloud size: "<<cloud_pedestrian_->size()<<endl;
    
    //Get min and max of pedestrian cloud
    pcl::getMinMax3D<pcl::PointXYZRGB>(*cloud_pedestrian_,min_pt,max_pt);

    model_.measured_height_=max_pt(1)-min_pt(1);
    //cout<<"Height: "<<model_.height_<<" m"<<endl;
    
    //Assume that the head belongs to the top points
    vector<double> limits_head = boost::assign::list_of<double>(-10)(10)(max_pt(1)-0.25)(max_pt(1))(0)(20);
    conditionalFilter(cloud_pedestrian_,model_.head.cloud_,limits_head);
    colormap.setColor(model_.head.cloud_,PedestrianModel::HEAD);
    
    //Get the top part of the cloud to speed up processing
    vector<double> limits_top_body = boost::assign::list_of<double>(-10)(10)(max_pt(1)-0.80)(max_pt(1))(0)(20);
    conditionalFilter(cloud_pedestrian_,cloud_top_body_,limits_top_body);
    colormap.setColor(cloud_top_body_,10);

    //Get head centroid
    Eigen::Vector4f head_centroid;
    pcl::compute3DCentroid<pcl::PointXYZRGB>(*model_.head.cloud_,head_centroid);
    model_.head.start_ = makePoint(head_centroid(0),head_centroid(1),head_centroid(2));
    model_.head.end_ = model_.head.start_;
    model_.head.detected_ = true;
    
    head_position_current_ = model_.head.start_;
    
    double knees_y_max_limit = max_pt(1)-model_.height_*(3./4.) + 0.15*model_.height_;
    double knees_y_min_limit = max_pt(1)-model_.height_*(3./4.) - 0.05*model_.height_;
    double knees_x_max_limit = (max_pt(0)+min_pt(0))/2.+1.0;
    double knees_x_min_limit = (max_pt(0)+min_pt(0))/2.-1.0;
    double knees_z_max_limit = (max_pt(2)+min_pt(2))/2.+1.0;
    double knees_z_min_limit = (max_pt(2)+min_pt(2))/2.-1.0;
    vector<double> limits_knees = boost::assign::list_of<double>(knees_x_min_limit)(knees_x_max_limit)(knees_y_min_limit)(knees_y_max_limit)(knees_z_min_limit)(knees_z_max_limit);
    conditionalFilter(cloud_pedestrian_,cloud_knees_,limits_knees);
    
    Eigen::Vector4f knees_centroid;
    
    if(cloud_knees_->size()>20)
    {
        model_.knees_midpoint_detected_ = true;
        pcl::compute3DCentroid<pcl::PointXYZRGB>(*cloud_knees_,knees_centroid);
    }else
        model_.knees_midpoint_detected_ = false;
    
    model_.motion_direction = head_position_current_ - head_position_preivous_;
    model_.main_velocity = norm(model_.motion_direction);
    model_.motion_direction = normalize(model_.motion_direction);
    
    cout<<"velocity: "<<model_.main_velocity<<endl;
    cout<<"head_position_current_: "<<head_position_current_<<endl;
    cout<<"head_position_preivous_: "<<head_position_preivous_<<endl;
    
    cout<<"locate neck"<<endl;
    model_.neck.sampling_.reset(new EllipseSampling);
    model_.neck.sampling_->start_point_=model_.head.end_;
    model_.neck.start_ = model_.head.end_;
    
    //Get neck preferencia position from main direction
    pcl::PointXYZRGB t = makePoint(0,1,0);
    pcl::PointXYZRGB v1 = makePoint(rotateByAxis(M_PI/2.,t.getVector3fMap(),model_.main_direction.getVector3fMap()));
    pcl::PointXYZRGB vd = makePoint(rotateByAxis(pcl::deg2rad(90.+20.),v1.getVector3fMap(),model_.main_direction.getVector3fMap()));
    normalize(vd);
    model_.neck.sampling_->preferencial_end_ = model_.head.end_ + vd*model_.getSegmentLength("neck");
            
    //model_.neck.sampling_->preferencial_end_ = model_.head.end_;
    //model_.neck.sampling_->preferencial_end_.y+=-model_.getSegmentLength("neck");
    model_.neck.sampling_->preferencial_direction_ = model_.motion_direction;
    model_.neck.sampling_->max_theta_=pcl::deg2rad(10.0);
    model_.neck.sampling_->max_phi_=pcl::deg2rad(10.0);
    model_.neck.sampling_->number_samples_ = 15;
    model_.neck.sampling_->createSamples();
    model_.neck.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_neck = model_.neck.sampling_->getBestSampleIndex();
    model_.neck.end_ = reinterpret_cast<EllipseSampling*>(model_.neck.sampling_.get())->samples_[max_indx_neck];
    model_.neck.detected_ = true;
    removeExplainedPoints(model_.neck.sampling_->distances_,max_indx_neck,0.1,cloud_top_body_,cloud_working,model_.neck.cloud_);
    colormap.setColor(model_.neck.cloud_,PedestrianModel::NECK);
    *cloud_accumulation_ += *model_.neck.cloud_;
    
    //Locate shoulders
    cout<<"locate shoulders"<<endl;
    
    model_.shoulders.sampling_.reset(new EllipseShapeSampling);
    model_.shoulders.sampling_->start_point_ = model_.neck.end_;
    model_.shoulders.start_ = model_.neck.end_;        
    reinterpret_cast<EllipseShapeSampling*>(model_.shoulders.sampling_.get())->a_ = model_.getSegmentLength("shoulder_width");
    reinterpret_cast<EllipseShapeSampling*>(model_.shoulders.sampling_.get())->b_ = model_.getSegmentLength("shoulder_depth");
    reinterpret_cast<EllipseShapeSampling*>(model_.shoulders.sampling_.get())->center_angle_ = M_PI + angleFromDirection(model_.main_direction);
    model_.shoulders.sampling_->max_theta_ = pcl::deg2rad(60.0);
    model_.shoulders.sampling_->createSamples();
    model_.shoulders.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_shoulders = model_.shoulders.sampling_->getBestSampleIndex();
    Ellipse::Ptr shoulder_best_sample = reinterpret_cast<EllipseShapeSampling*>(model_.shoulders.sampling_.get())->samples_[max_indx_shoulders];
    
    model_.shoulders.ellipse_ = shoulder_best_sample;
    model_.shoulders.left_ = makePoint(model_.shoulders.ellipse_->a_0_);
    model_.shoulders.right_ = makePoint(model_.shoulders.ellipse_->a_pi_);
    model_.shoulders.detected_ = true;
    
    model_.shoulder_direction = makePoint(model_.shoulders.ellipse_->v_)*-1;
    //double shoulder_angle = angleFromDirection(model_.shoulder_direction);
    
    double cf = min(model_.main_velocity/0.3,1.);
    
    model_.main_direction = (1-cf)*model_.shoulder_direction + cf*model_.motion_direction;
    //model_.main_direction = normalize(model_.motion_direction);
    model_.main_direction = normalize(model_.main_direction);
    
    removeExplainedPoints(model_.shoulders.sampling_->distances_,max_indx_shoulders,0.1,cloud_top_body_,cloud_working,model_.shoulders.cloud_);
    colormap.setColor(model_.shoulders.cloud_,PedestrianModel::SHOULDERS);
    *cloud_accumulation_ += *model_.shoulders.cloud_;
    
    //Locate upper arms using standard segmentation
    //upperArmIndependentSegmentation();
    
    //Locate lower arms using standard segmentation
    //lowerArmIndependentSegmentation();
            
    //Copy to removed arms if we are not detecting them
    if(!model_.left_upper_arm.detected_ || !model_.right_lower_arm.detected_)
        pcl::copyPointCloud (*cloud_pedestrian_,*cloud_removed_arms_);
            
    cout<<"locate upper_torso"<<endl;
    model_.upper_torso.sampling_.reset(new DistortedSphereSampling);
    model_.upper_torso.sampling_->start_point_ = model_.neck.end_;
    model_.upper_torso.start_ = model_.neck.end_;
    model_.upper_torso.sampling_->preferencial_end_ = model_.neck.end_;
    model_.upper_torso.sampling_->preferencial_end_.y+=-model_.getSegmentLength("torso")/2.;
    model_.upper_torso.sampling_->preferencial_direction_ = model_.main_direction;
    model_.upper_torso.sampling_->max_theta_=pcl::deg2rad(15.0);
    model_.upper_torso.sampling_->max_phi_=pcl::deg2rad(15.0);
    model_.upper_torso.sampling_->number_samples_ = 50;
    reinterpret_cast<DistortedSphereSampling*>(model_.upper_torso.sampling_.get())->compression_factor_=1.5;
    model_.upper_torso.sampling_->weibull_delta_ = model_.getSegmentLength("hip")/1.2;
    model_.upper_torso.sampling_->weibull_k_ = 3.0;
    cout<<"weibull_delta_: "<<model_.getSegmentLength("hip")/1.2<<endl;
    
    model_.upper_torso.sampling_->createSamples();
    model_.upper_torso.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_upper_torso = model_.upper_torso.sampling_->getBestSampleIndex();
    model_.upper_torso.end_ = reinterpret_cast<DistortedSphereSampling*>(model_.upper_torso.sampling_.get())->samples_[max_indx_upper_torso];
    
    if(model_.knees_midpoint_detected_)
    {
        model_.upper_torso.end_.x = (model_.upper_torso.end_.x + knees_centroid(0))/2.;
        model_.upper_torso.end_.z = (model_.upper_torso.end_.z + knees_centroid(2))/2.;
    }
    
    model_.upper_torso.detected_ = true;
    removeExplainedPoints(model_.upper_torso.sampling_->distances_,max_indx_upper_torso,0.2,cloud_top_body_,cloud_working,model_.upper_torso.cloud_);
    colormap.setColor(model_.upper_torso.cloud_,PedestrianModel::WAIST);
    *cloud_accumulation_ += *model_.upper_torso.cloud_;
    
    cout<<"locate bottom torso"<<endl;
    model_.torso.sampling_.reset(new DistortedSphereSampling);
    model_.torso.sampling_->start_point_ = model_.upper_torso.end_;
    model_.torso.start_ = model_.upper_torso.end_;
    model_.torso.sampling_->preferencial_end_ = model_.upper_torso.end_;
    model_.torso.sampling_->preferencial_end_.y+=-model_.getSegmentLength("torso")/2.;
    model_.torso.sampling_->preferencial_direction_ = model_.main_direction;
    model_.torso.sampling_->max_theta_=pcl::deg2rad(15.0);
    model_.torso.sampling_->max_phi_=pcl::deg2rad(8.0);
    model_.torso.sampling_->number_samples_ = 50;
    reinterpret_cast<DistortedSphereSampling*>(model_.torso.sampling_.get())->compression_factor_=1.5;
    model_.torso.sampling_->weibull_k_ = 2.0;
    model_.torso.sampling_->weibull_delta_ = model_.getSegmentLength("hip")/1.5;
    cout<<"weibull_delta_: "<<model_.getSegmentLength("hip")/1.5<<endl;
    
    model_.torso.sampling_->createSamples();
    model_.torso.sampling_->scoreSamples(cloud_removed_arms_);
    
    int max_indx_waist = model_.torso.sampling_->getBestSampleIndex();
    
    model_.torso.end_ = reinterpret_cast<DistortedSphereSampling*>(model_.torso.sampling_.get())->samples_[max_indx_waist];
    
    if(model_.knees_midpoint_detected_)
    {
        model_.torso.end_.x = (model_.torso.end_.x + knees_centroid(0))/2.;
        model_.torso.end_.z = (model_.torso.end_.z + knees_centroid(2))/2.;
    }
    
    model_.torso.detected_ = true;
    removeExplainedPoints(model_.torso.sampling_->distances_,max_indx_waist,0.2,cloud_removed_arms_,cloud_working,model_.torso.cloud_);
    colormap.setColor(model_.torso.cloud_,PedestrianModel::WAIST);
    *cloud_accumulation_ += *model_.torso.cloud_;

    //Get cloud bellow waist
    vector<double> bellow_waist_limits = boost::assign::list_of<double>(-10)(10)(-10)(model_.torso.end_.y)(0)(20);
    conditionalFilter(cloud_removed_arms_,cloud_bellow_waist_,bellow_waist_limits);

    cout<<"locate center torso"<<endl;
    model_.center_torso.sampling_.reset(new RotationOnlySampling);
    model_.center_torso.sampling_->start_point_ = model_.upper_torso.end_;
    model_.center_torso.start_ = model_.center_torso.sampling_->start_point_;
    model_.center_torso.end_=model_.center_torso.start_;
    
    reinterpret_cast<RotationOnlySampling*>(model_.center_torso.sampling_.get())->length_ = model_.getSegmentLength("center_torso_width");
    reinterpret_cast<RotationOnlySampling*>(model_.center_torso.sampling_.get())->center_angle_ = angleFromDirection(model_.main_direction);
    model_.center_torso.sampling_->max_theta_ = pcl::deg2rad(50.0);
    model_.center_torso.sampling_->createSamples();
    model_.center_torso.sampling_->scoreSamples(cloud_top_body_);
    
    int max_indx_center_torso = model_.center_torso.sampling_->getBestSampleIndex();
    Eigen::MatrixXf center_torso_best_sample;
    center_torso_best_sample.resize(1,6);
    center_torso_best_sample = reinterpret_cast<RotationOnlySampling*>(model_.center_torso.sampling_.get())->samples_.row(max_indx_center_torso);
    
    model_.center_torso.left_ = makePoint(center_torso_best_sample(0),center_torso_best_sample(1),center_torso_best_sample(2));
    model_.center_torso.right_ = makePoint(center_torso_best_sample(3),center_torso_best_sample(4),center_torso_best_sample(5));
    model_.center_torso.detected_ = true;
    
    removeExplainedPoints(model_.center_torso.sampling_->distances_,max_indx_center_torso,0.1,cloud_top_body_,cloud_working,model_.center_torso.cloud_);
    colormap.setColor(model_.center_torso.cloud_,PedestrianModel::CENTER_TORSO);
    *cloud_accumulation_ += *model_.center_torso.cloud_;
    
    cout<<"locate hips"<<endl;
    model_.hips.sampling_.reset(new EllipseShapeSampling);
    model_.hips.sampling_->start_point_ = model_.torso.end_;
    
    reinterpret_cast<EllipseShapeSampling*>(model_.hips.sampling_.get())->a_ = model_.getSegmentLength("hip")/2.;
    reinterpret_cast<EllipseShapeSampling*>(model_.hips.sampling_.get())->b_ = model_.getSegmentLength("hip")/4.;
    reinterpret_cast<EllipseShapeSampling*>(model_.hips.sampling_.get())->center_angle_ = M_PI + angleFromDirection(model_.main_direction);
    model_.hips.sampling_->max_theta_ = pcl::deg2rad(30.0);
    
    //reinterpret_cast<RotationOnlySampling*>(model_.hips.sampling_.get())->length_ = model_.getSegmentLength("hip");
    //reinterpret_cast<RotationOnlySampling*>(model_.hips.sampling_.get())->center_angle_ = angleFromDirection(model_.main_direction);
    //model_.hips.sampling_->max_theta_ = pcl::deg2rad(30.0);
    model_.hips.sampling_->createSamples();
    model_.hips.sampling_->scoreSamples(cloud_pedestrian_);
    
    int max_indx_hip = model_.hips.sampling_->getBestSampleIndex();
    Ellipse::Ptr hip_best_sample = reinterpret_cast<EllipseShapeSampling*>(model_.hips.sampling_.get())->samples_[max_indx_hip];
    
    model_.hips.ellipse_ = hip_best_sample;
    model_.hips.left_ = makePoint(model_.hips.ellipse_->a_0_);
    model_.hips.right_ = makePoint(model_.hips.ellipse_->a_pi_);
    model_.hips.detected_ = true;
    
    //int max_indx_hip = model_.hips.sampling_->getBestSampleIndex();
    //Eigen::MatrixXf hip_best_sample;
    //hip_best_sample.resize(1,6);
    //hip_best_sample = reinterpret_cast<RotationOnlySampling*>(model_.hips.sampling_.get())->samples_.row(max_indx_hip);
    
    //model_.hips.left_ = makePoint(hip_best_sample(0),hip_best_sample(1),hip_best_sample(2));
    //model_.hips.right_ = makePoint(hip_best_sample(3),hip_best_sample(4),hip_best_sample(5));
    //model_.hips.detected_ = true;
    
    //Segment upper legs
    upperLegComposedSegmentation();

    //Left leg parametric minimization scheme
    
    model_.left_foot.minimization.cloud = cloud_bellow_knees_;
    model_.left_foot.minimization.main_direction = model_.main_direction.getVector3fMap();
    model_.left_foot.minimization.fixed_point = model_.left_leg.end_.getVector3fMap();
    
    
    //Composed 2 fold foot location
    //Locate best left and right feet using a cloud without knees
    cout<<"locate left foot"<<endl;
    model_.left_foot.sampling_.reset(new OffCenterEllipseSampling);
    model_.left_foot.sampling_->start_point_ = model_.left_leg.end_;
    model_.left_foot.sampling_->number_samples_ = 150;
    model_.left_foot.start_ = model_.left_leg.end_;
    
    Eigen::Vector3f left_foot_direction = model_.left_leg.end_.getVector3fMap() - model_.left_leg.start_.getVector3fMap();
    left_foot_direction.normalize();
    
    Eigen::Vector3f left_foot_position = model_.left_foot.start_.getVector3fMap()+model_.getSegmentLength("lower_leg")*left_foot_direction;
    model_.left_foot.sampling_->preferencial_end_ = makePoint(left_foot_position);
    
    model_.left_foot.minimization.preferencial_end = left_foot_position;
    
    if(model_.left_foot.minimization.cloud->points.size()>0)
    {
        //cout<<"starting minimization"<<endl;
        model_.left_foot.minimization.minimize();
        //cout<<"done"<<endl;
        //exit(0);
        
        model_.left_foot.end_ = model_.left_foot.minimization.end_point;
        model_.left_foot.detected_ = true;
        
        head_position_preivous_ = head_position_current_;
        return;
    }
    
    
    //model_.left_foot.sampling_->preferencial_direction_ = makePoint(0,0,-1);
    model_.left_foot.sampling_->preferencial_direction_ = model_.main_direction;
    model_.left_foot.sampling_->max_theta_=pcl::deg2rad(60.0);
    model_.left_foot.sampling_->max_phi_=pcl::deg2rad(30.0);
    reinterpret_cast<OffCenterEllipseSampling*>(model_.left_foot.sampling_.get())->off_center_ = pcl::deg2rad(-45.0);
    
    model_.left_foot.sampling_->createSamples();
    model_.left_foot.sampling_->scoreSamples(cloud_bellow_knees_);
    
    int max_indx_left_foot = model_.left_foot.sampling_->getBestSampleIndex();
    double max_score_left_foot = model_.left_foot.sampling_->scores_[max_indx_left_foot];
    pcl::PointXYZRGB p_left_foot_trial = reinterpret_cast<OffCenterEllipseSampling*>(model_.left_foot.sampling_.get())->samples_[max_indx_left_foot];
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_knees_left_foot_removed(new pcl::PointCloud<pcl::PointXYZRGB>);
    removeExplainedPoints(model_.left_foot.sampling_->distances_,max_indx_left_foot,0.10,cloud_bellow_knees_,cloud_bellow_knees_left_foot_removed,cloud_auxiliar);
    
    
    cout<<"locate right foot"<<endl;
    model_.right_foot.sampling_.reset(new OffCenterEllipseSampling);
    model_.right_foot.sampling_->start_point_ = model_.right_leg.end_;
    model_.right_foot.sampling_->number_samples_ = 150;
    model_.right_foot.start_ = model_.right_leg.end_;
    
    Eigen::Vector3f right_foot_direction = model_.right_leg.end_.getVector3fMap() - model_.right_leg.start_.getVector3fMap();
    right_foot_direction.normalize();
    
    Eigen::Vector3f right_foot_position = model_.right_foot.start_.getVector3fMap()+model_.getSegmentLength("lower_leg")*right_foot_direction;
    model_.right_foot.sampling_->preferencial_end_ = makePoint(right_foot_position);
    //model_.right_foot.sampling_->preferencial_direction_ = makePoint(0,0,-1);
    model_.right_foot.sampling_->preferencial_direction_ = model_.main_direction;
    model_.right_foot.sampling_->max_theta_=pcl::deg2rad(60.0);
    model_.right_foot.sampling_->max_phi_=pcl::deg2rad(30.0);
    reinterpret_cast<OffCenterEllipseSampling*>(model_.right_foot.sampling_.get())->off_center_ = pcl::deg2rad(-45.0);
    
    model_.right_foot.sampling_->createSamples();
    model_.right_foot.sampling_->scoreSamples(cloud_bellow_knees_);
    
    int max_indx_right_foot = model_.right_foot.sampling_->getBestSampleIndex();
    double max_score_right_foot = model_.right_foot.sampling_->scores_[max_indx_right_foot];
    pcl::PointXYZRGB p_right_foot_trial = reinterpret_cast<OffCenterEllipseSampling*>(model_.right_foot.sampling_.get())->samples_[max_indx_right_foot];
    
    cout<<"max right foot located, creating segmented cloud"<<endl;
    cout<<"max_indx_right_foot: "<<max_indx_right_foot<<endl;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_bellow_knees_right_foot_removed(new pcl::PointCloud<pcl::PointXYZRGB>);
    removeExplainedPoints(model_.right_foot.sampling_->distances_,max_indx_right_foot,0.10,cloud_bellow_knees_,cloud_bellow_knees_right_foot_removed,cloud_auxiliar);
    
    //Now check witch one is best

    cout<<"left foot score: "<<max_score_left_foot<<endl;
    cout<<"right foot score: "<<max_score_right_foot<<endl;

    //If left foot is better, remove points and detect the right foot
    if(max_score_left_foot>max_score_right_foot)
    {
        cout<<"left foot is better"<<endl;
        
        excludeSamplesByProximityToLineSegment(model_.left_foot.start_,p_left_foot_trial,reinterpret_cast<OffCenterEllipseSampling*>(model_.right_foot.sampling_.get())->samples_,0.1);
        model_.right_foot.sampling_->scoreSamples(cloud_bellow_knees_left_foot_removed);
        max_indx_right_foot = model_.right_foot.sampling_->getBestSampleIndex();
        
        model_.right_foot.end_ = reinterpret_cast<OffCenterEllipseSampling*>(model_.right_foot.sampling_.get())->samples_[max_indx_right_foot];
        model_.left_foot.end_ = p_left_foot_trial;
        
        cloud_bellow_knees_left_foot_removed->clear();
        removeExplainedPoints(model_.left_foot.sampling_->distances_,max_indx_left_foot,0.10,cloud_bellow_knees_,cloud_bellow_knees_left_foot_removed,model_.left_foot.cloud_);
        colormap.setColor(model_.left_foot.cloud_,PedestrianModel::LEFT_FOOT);
        *cloud_accumulation_ += *model_.left_foot.cloud_;

        //The point cloud cloud_bellow_knees_left_foot_removed, changed from the previous step, i need to correct the distances matrix (or recalculate it)
        model_.right_foot.sampling_->scoreSamples(cloud_bellow_knees_left_foot_removed);
        
        removeExplainedPoints(model_.right_foot.sampling_->distances_,max_indx_right_foot,0.10,cloud_bellow_knees_left_foot_removed,cloud_working,model_.right_foot.cloud_);
        colormap.setColor(model_.right_foot.cloud_,PedestrianModel::RIGHT_FOOT);
        *cloud_accumulation_ += *model_.right_foot.cloud_;
        
    }else//right foot fits best
    {
        cout<<"right foot is better"<<endl;

        excludeSamplesByProximityToLineSegment(model_.right_foot.start_,p_right_foot_trial,reinterpret_cast<OffCenterEllipseSampling*>(model_.left_foot.sampling_.get())->samples_,0.1);
        model_.left_foot.sampling_->scoreSamples(cloud_bellow_knees_right_foot_removed);
        max_indx_left_foot = model_.left_foot.sampling_->getBestSampleIndex();

        model_.left_foot.end_ = reinterpret_cast<OffCenterEllipseSampling*>(model_.left_foot.sampling_.get())->samples_[max_indx_left_foot];
        model_.right_foot.end_ = p_right_foot_trial;
        
        
        cloud_bellow_knees_right_foot_removed->clear();
        removeExplainedPoints(model_.right_foot.sampling_->distances_,max_indx_right_foot,0.10,cloud_bellow_knees_,cloud_bellow_knees_right_foot_removed,model_.right_foot.cloud_);
        colormap.setColor(model_.right_foot.cloud_,PedestrianModel::RIGHT_FOOT);
        *cloud_accumulation_ += *model_.right_foot.cloud_;
        
        //The point cloud cloud_bellow_knees_right_foot_removed, changed from the previous step, i need to correct the distances matrix (or recalculate it)
        model_.left_foot.sampling_->scoreSamples(cloud_bellow_knees_right_foot_removed);
        
        removeExplainedPoints(model_.left_foot.sampling_->distances_,max_indx_left_foot,0.10,cloud_bellow_knees_right_foot_removed,cloud_working,model_.left_foot.cloud_);
        colormap.setColor(model_.left_foot.cloud_,PedestrianModel::LEFT_FOOT);
        *cloud_accumulation_ += *model_.left_foot.cloud_;
    }
    
    model_.left_foot.detected_ = true;
    model_.right_foot.detected_ = true;
    
    head_position_preivous_ = head_position_current_;
}