text:
#include "shape_reconstruction/ShapeReconstruction.h"

#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter_indices.h>
#include <sensor_msgs/Image.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <pcl_conversions/pcl_conversions.h>

#include <ros/package.h>

#include <cmath>

#include <ros/console.h>

#include "shape_reconstruction/RangeImagePlanar.hpp"

#include "shape_reconstruction/Passthrough.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>

#include <boost/lexical_cast.hpp>
#include <sensor_msgs/image_encodings.h>

#include <pcl/point_types_conversion.h>

#include <pcl/registration/icp.h>

#include <pcl/common/centroid.h>

#include <pcl/surface/poisson.h>
#include <pcl/surface/mls.h>

#include <math.h>

using namespace omip;

ShapeReconstruction::ShapeReconstruction()
    :
      _accumulate_change_candidates(true),
      _min_depth_change(0.08),
      _remove_inconsistent_points(true),
      _t(0),
      _extend_to_neighbor_sv(false),
      _acc_candidates_of_current(IMG_HEIGHT, IMG_WIDTH, CV_8UC1),
      _similarity_in_normal(0),
      _similarity_in_h(0)
{
    // Create auxiliary depth map image
    this->_acc_candidates_of_current.setTo(0);

    // Create range image planar object
    this->_rip_temp = pcl::RangeImagePlanar::Ptr(new pcl::RangeImagePlanar());

    // Create indices
    this->_moving_pts_of_current_idx_depth = pcl::PointIndices::Ptr(new pcl::PointIndices ());
    this->_moving_pts_of_previous_idx_depth = pcl::PointIndices::Ptr(new pcl::PointIndices ());
    this->_out_of_view_pts_of_current_idx= pcl::PointIndices::Ptr(new pcl::PointIndices ());
    this->_occluded_pts_of_current_idx= pcl::PointIndices::Ptr(new pcl::PointIndices ());

    this->_moving_pts_of_current_idx_color = pcl::PointIndices::Ptr(new pcl::PointIndices ());
    this->_moving_pts_of_previous_idx_color = pcl::PointIndices::Ptr(new pcl::PointIndices ());

    this->_knn = pcl::search::Search<omip::SRPoint>::Ptr(new pcl::search::KdTree<omip::SRPoint>);

    this->_rb_shape  =  omip::SRPointCloud::Ptr(new omip::SRPointCloud);
    this->_rb_shape->height=1;

    this->_supporting_features = pcl::PointCloud<pcl::PointXYZL>::Ptr(new pcl::PointCloud<pcl::PointXYZL>());

    this->_candidates = SRPointCloud::Ptr(new SRPointCloud);
    this->_candidates->height=1;

    this->_candidates_in_current = SRPointCloud::Ptr(new SRPointCloud);

    this->_rb_segment.reset(new SRPointCloud);

    /// Mesh generation and storage as STL file
    ///
    /// For RB shape
    this->_rb_polygon_data_ptr = vtkSmartPointer<vtkPolyData>::New ();
    this->_rb_normal_estimator_ptr.reset(new pcl::NormalEstimationOMP<omip::SRPoint, pcl::Normal>());
    this->_rb_normal_estimator_ptr->setKSearch (50);
    this->_rb_estimated_normals_pc_ptr.reset(new pcl::PointCloud<pcl::Normal>());
    this->_rb_tree_for_normal_estimation_ptr.reset(new pcl::search::KdTree<omip::SRPoint>());
    this->_rb_position_color_and_normals_pc_ptr.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
    this->_rb_tree_for_triangulation_ptr.reset(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
    this->_rb_greedy_projection_triangulator_ptr.reset(new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>());
    // Set typical values for the parameters
    this->_rb_greedy_projection_triangulator_ptr->setMu (2.5);
    this->_rb_greedy_projection_triangulator_ptr->setMaximumNearestNeighbors (100);
    this->_rb_greedy_projection_triangulator_ptr->setMaximumSurfaceAngle(M_PI); // 180 degrees
    this->_rb_greedy_projection_triangulator_ptr->setMinimumAngle(0.0); // 5 degrees
    this->_rb_greedy_projection_triangulator_ptr->setMaximumAngle(M_PI); // 180 degrees
    this->_rb_greedy_projection_triangulator_ptr->setNormalConsistency(true);
    this->_rb_greedy_projection_triangulator_ptr->setConsistentVertexOrdering(true);
    // Set the maximum distance between connected points (maximum edge length)
    this->_rb_greedy_projection_triangulator_ptr->setSearchRadius (0.1);
    this->_rb_polygon_writer_ptr = vtkSmartPointer<vtkSTLWriter>::New ();
    this->_rb_polygon_writer_ptr->SetFileTypeToBinary();
    this->_rb_triangulated_mesh_ptr.reset(new pcl::PolygonMesh());
}

void ShapeReconstruction::setInitialFullRGBDPCAndRBT(const SRPointCloud::Ptr &initial_pc_msg,
                                                     const geometry_msgs::TwistWithCovariance &rb_transformation_initial)
{
    // Copy the initial point cloud
    pcl::copyPointCloud(*initial_pc_msg, *this->_initial_ffs._pc);

    // Get time stamp of the initial point cloud
    this->_initial_ffs._time.fromNSec(initial_pc_msg->header.stamp);

    // Clean the point cloud of nans
    pcl::removeNaNFromPointCloud<SRPoint>(*this->_initial_ffs._pc,*this->_initial_ffs._pc_without_nans, this->_initial_ffs._not_nan_indices);

    // Get the RGB image from the point cloud
    pcl::toROSMsg (*initial_pc_msg, this->_initial_ffs._rgb); //convert the cloud

    // Get the initial transformation
    this->_initial_ffs._transformation = geometry_msgs::TwistWithCovariance(rb_transformation_initial);

    // Get the depth map from the point cloud
    OrganizedPC2DepthMap(this->_initial_ffs._pc, this->_initial_ffs._dm->image);

    if(this->_depth_filling)
    {
        fillNaNsCBF(this->_initial_ffs._rgb, this->_initial_ffs._dm->image, this->_initial_ffs._dm_filled, this->_initial_ffs._time);
    }
}

void ShapeReconstruction::initialize()
{
    std::stringstream pub_topic_name_ext_d_and_c;
    pub_topic_name_ext_d_and_c << "/shape_recons/shape_rb" << this->_rb_id;
    this->_rb_shape_pub = this->_node_handle.advertise<sensor_msgs::PointCloud2>(pub_topic_name_ext_d_and_c.str(), 1);

    std::stringstream pub_segment_topic_name;
    pub_segment_topic_name << "/shape_recons/segment_rb" << this->_rb_id;
    this->_rb_segment_pub = this->_node_handle.advertise<sensor_msgs::PointCloud2>(pub_segment_topic_name.str(), 1);

    std::stringstream pub_occlusion_topic_name;
    pub_occlusion_topic_name << "/shape_recons/pc_occlusions_rb" << this->_rb_id;
    this->_occlusions_pub = this->_node_handle.advertise<sensor_msgs::PointCloud2>(pub_occlusion_topic_name.str(), 1);

    std::stringstream pub_occlusion_t_topic_name;
    pub_occlusion_t_topic_name << "/shape_recons/pc_occlusions_transf_rb" << this->_rb_id;
    this->_occlusions_transformed_pub = this->_node_handle.advertise<sensor_msgs::PointCloud2>(pub_occlusion_t_topic_name.str(), 1);

    this->_approximate_voxel_grid_filter.setLeafSize(this->_leaf_size,
                                                     this->_leaf_size,
                                                     this->_leaf_size);

    // This filter should delete single super voxels. If the properties of the super voxels
    // change, or the leaf size of the voxel grid filter change, the radius and/or the min neighbors of
    // the outlier removal should change
    // set radius for neighbor search
    this->_radius_outlier_removal.setRadiusSearch (this->_ror_radius_search);
    // set threshold for minimum required neighbors neighbors
    this->_radius_outlier_removal.setMinNeighborsInRadius (this->_ror_min_neighbors);

    if(this->_record_videos)
    {
        std::string videos_path = ros::package::getPath("shape_reconstruction") + std::string("/videos/");

        // create folder if does not exist
        boost::filesystem::path videos_folder(videos_path);
        if (!boost::filesystem::exists(videos_folder)) {
            if (!boost::filesystem::create_directories(videos_folder)) {
                ROS_ERROR_NAMED("ShapeReconstruction.ShapeReconstruction", "Output directory for video bag does not exist and cannot be created!");
                return;
            }
        }

        this->_videos.open(videos_path + std::string("videos_rb") + boost::lexical_cast<std::string>(this->_rb_id)+std::string(".bag"),rosbag::bagmode::Write);

        sensor_msgs::ImagePtr initial_dm_msg;
        DepthImage2CvImage(this->_initial_ffs._dm->image, initial_dm_msg);
        this->_videos.write("original_dm", this->_initial_ffs._time, initial_dm_msg);

        if(this->_depth_filling)
        {
            sensor_msgs::ImagePtr initial_dm_filled_msg;
            DepthImage2CvImage(this->_initial_ffs._dm_filled, initial_dm_filled_msg);
            this->_videos.write("filled_dm", this->_initial_ffs._time, initial_dm_filled_msg);
        }
    }
}

ShapeReconstruction::ShapeReconstruction(const ShapeReconstruction &sr)
{
    ROS_ERROR_STREAM_NAMED("ShapeReconstruction.ShapeReconstruction", "Do not use this copy constructor! It is not complete!");

    this->_rb_id = sr._rb_id;

    this->_to_initial = sr._to_initial;
    this->_depth_filling = sr._depth_filling;

    this->_initial_ffs = sr._initial_ffs.clone();
    this->_previous_ffs = sr._previous_ffs.clone();
    this->_current_ffs = sr._current_ffs.clone();

    this->_rip_temp = pcl::RangeImagePlanar::Ptr(new pcl::RangeImagePlanar(*sr._rip_temp));

    this->_moving_pts_of_current_idx_depth = pcl::PointIndices::Ptr(new pcl::PointIndices(*sr._moving_pts_of_current_idx_depth));
    this->_moving_pts_of_previous_idx_depth = pcl::PointIndices::Ptr(new pcl::PointIndices(*sr._moving_pts_of_previous_idx_depth));
    this->_out_of_view_pts_of_current_idx = pcl::PointIndices::Ptr(new pcl::PointIndices(*sr._out_of_view_pts_of_current_idx));
    this->_occluded_pts_of_current_idx = pcl::PointIndices::Ptr(new pcl::PointIndices(*sr._occluded_pts_of_current_idx));

}

ShapeReconstruction::~ShapeReconstruction()
{
    if(this->_record_videos)
    {
        _videos.close();
    }
}

void ShapeReconstruction::setCameraInfo(const sensor_msgs::CameraInfo& camera_info)
{
    this->_ci = sensor_msgs::CameraInfo(camera_info);
}

void ShapeReconstruction::setFullRGBDPCandRBT(const SRPointCloud::Ptr &pc_msg,
                                              const geometry_msgs::TwistWithCovariance &rb_transformation)
{
    ros::Time t1 = ros::Time::now();

    _previous_k_indices.clear();
    _current_k_indices.clear();
    // t=0 means first point cloud AFTER initial received
    ROS_WARN_STREAM_NAMED("ShapeReconstruction.setFullRGBDPCandRBT", "[RB" << this->_rb_id << "]: t = " << _t);

    if(this->_current_ffs._pc->points.size() > 0 )
    {
        this->_previous_ffs = this->_current_ffs;
    }else{
        this->_previous_ffs = this->_initial_ffs;
    }
    this->_current_ffs.reset();

    ros::Time t2 = ros::Time::now();

    pcl::copyPointCloud(*pc_msg, *this->_current_ffs._pc);

    ros::Time t3 = ros::Time::now();

    // Clean the point cloud of nans
    this->_current_ffs._not_nan_indices.clear();
    pcl::removeNaNFromPointCloud<SRPoint>(*this->_current_ffs._pc,*this->_current_ffs._pc_without_nans, this->_current_ffs._not_nan_indices);

    ros::Time t4 = ros::Time::now();

    ROS_WARN_STREAM_NAMED("ShapeReconstruction.setFullRGBDPCandRBT", "[RB" << this->_rb_id << "]: " << "Point cloud time stamp: "
                          <<this->_current_ffs._pc->header.stamp <<  ", not nan points "
                          << this->_current_ffs._not_nan_indices.size() << "/" << this->_current_ffs._pc_without_nans->size());

    this->_current_ffs._time = pcl_conversions::fromPCL(pc_msg->header.stamp);
    pcl::toROSMsg (*pc_msg, this->_current_ffs._rgb); //convert the cloud
    this->_current_ffs._transformation = geometry_msgs::TwistWithCovariance(rb_transformation);

    ros::Time t5 = ros::Time::now();

    // Create a depth map from the current organized depth map
    OrganizedPC2DepthMap(this->_current_ffs._pc, this->_current_ffs._dm->image);

    ros::Time t6 = ros::Time::now();

    // -----------------------------------------------------------
    // main algorithm

    if(this->_depth_filling)
    {
        fillNaNsCBF(this->_current_ffs._rgb, this->_current_ffs._dm->image, this->_current_ffs._dm_filled, this->_current_ffs._time);
        this->_current_ffs._dm->image.copyTo(this->_current_ffs._dm_filled, this->_current_ffs._dm->image == this->_current_ffs._dm->image);
    }

    ros::Time t7 = ros::Time::now();

    if(this->_record_videos)
    {
        sensor_msgs::ImagePtr current_dm_msg;
        DepthImage2CvImage(this->_current_ffs._dm->image, current_dm_msg);
        this->_videos.write("original_dm", this->_current_ffs._time, current_dm_msg);

        if(this->_depth_filling)
        {
            sensor_msgs::ImagePtr current_dm_filled_msg;
            DepthImage2CvImage(this->_current_ffs._dm_filled, current_dm_filled_msg);
            this->_videos.write("filled_dm", this->_current_ffs._time, current_dm_filled_msg);
        }
    }

    // Segmentation method 1: motion segmentation based on DEPTH (memoryless)
    this->_DetectImageLocationsWhereDepthChanges();
    ros::Time t8 = ros::Time::now();
    this->_TestMotionCoherencyOfPointsInImageLocationsWhereDepthChanged();
    ros::Time t9 = ros::Time::now();

    // Segmentation method 2: motion segmentation based on COLOR (memoryless)
    this->_DetectImageLocationsWhereColorChanges();
    ros::Time t10 = ros::Time::now();
    this->_TestMotionCoherencyOfPointsInImageLocationsWhereColorChanged();
    ros::Time t11 = ros::Time::now();

    // Merge the segmentation results into the model(s)
    this->_MergeValidPointsIntoModels();

    ros::Time t12 = ros::Time::now();

    this->_RemoveInconsistentPointsFromModelsAndExtendToRegions();

    ros::Time t12b = ros::Time::now();

    this->_FilterModel();

    ros::Time t13 = ros::Time::now();

    _t++;

//    std::cout << "Preprocessing: " << (t7-t1).toSec() << std::endl;
//    std::cout << "Depth-based segmentation: " << (t9-t7).toSec() << std::endl;
//    std::cout << "Color-based segmentation: " << (t11-t9).toSec() << std::endl;
//    std::cout << "Remove inconsistent points and extend to regions: " << (t12b-t12).toSec() << std::endl;
//    std::cout << "Filter model: " << (t13-t12b).toSec() << std::endl;
//    std::cout << "Total processing RGBD frame (one RB): " << (t13-t1).toSec() << std::endl;
}

void ShapeReconstruction::_DetectImageLocationsWhereDepthChanges()
{
    // Find the difference
    if(this->_depth_filling)
    {
        this->_difference_in_depth = this->_current_ffs._dm_filled - this->_previous_ffs._dm_filled;
    }else{
        this->_difference_in_depth = this->_current_ffs._dm->image - this->_previous_ffs._dm->image;
    }

    if(this->_record_videos)
    {
        sensor_msgs::ImagePtr dm_msg;
        DepthImage2CvImage(_difference_in_depth, dm_msg);
        this->_videos.write("changes_in_depth", this->_current_ffs._time, dm_msg);
    }

    // If the difference is negative (under some threshold) that means that the object occludes now what was behind and visible before
    // This part of the CURRENT depth map (of the CURRENT point cloud) should be added to the candidates of the RB
    cv::threshold(this->_difference_in_depth, this->_candidates_of_current, -_min_depth_change, 255, cv::THRESH_BINARY_INV);

    if(this->_record_videos)
    {
        sensor_msgs::ImagePtr dm_msg;
        DepthImage2CvImage(_candidates_of_current, dm_msg);
        this->_videos.write("candidates_of_current", this->_current_ffs._time, dm_msg);
    }

    // If the difference is positive (over some threshold) that means that the object is now not occluding what it was occluding before
    // This part of the PREVIOUS depth map (of the PREVIOUS point cloud) should be added to the candidates of the RB
    cv::threshold(this->_difference_in_depth, this->_candidates_of_previous, _min_depth_change, 255, cv::THRESH_BINARY);

    if(this->_record_videos)
    {
        sensor_msgs::ImagePtr dm_msg;
        DepthImage2CvImage(_candidates_of_previous, dm_msg);
        this->_videos.write("candidates_of_previous", this->_current_ffs._time, dm_msg);
    }

    this->_previous_depth_mask = this->_previous_ffs._dm->image > 1.5 | this->_previous_ffs._dm->image < 0.3 | this->_previous_ffs._dm->image != this->_previous_ffs._dm->image;
    this->_current_depth_mask = this->_current_ffs._dm->image > 1.5 | this->_current_ffs._dm->image < 0.3 | this->_current_ffs._dm->image != this->_current_ffs._dm->image;

    this->_candidates_of_current.convertTo(this->_candidates_of_current_8u, CV_8U);
    this->_candidates_of_previous.convertTo(this->_candidates_of_previous_8u, CV_8U);

    // accumulate candidates
    this->_acc_candidates_of_current = _acc_candidates_of_current | this->_candidates_of_current_8u;
    this->_acc_candidates_of_current.setTo(0,this->_candidates_of_previous_8u);

    if(this->_record_videos)
    {
        sensor_msgs::ImagePtr dm_msg;
        DepthImage2CvImage(_acc_candidates_of_current, dm_msg);
        this->_videos.write("acc_candidates_of_current", this->_current_ffs._time, dm_msg);
    }

    if (!this->_depth_filling)
    {
        ROS_INFO_STREAM_NAMED("ShapeReconstruction._DetectImageLocationsWhereDepthChanges",  "[RB" << this->_rb_id << "]: Removing nan points from accumulator");
        this->_acc_candidates_of_current.setTo(0, this->_current_ffs._dm->image != this->_current_ffs._dm->image);
    }

    if(this->_accumulate_change_candidates)
    {
        this->_acc_candidates_of_current.copyTo(this->_candidates_of_current_8u);
    }

    this->_candidates_of_current_8u.setTo(0, this->_current_depth_mask);
    this->_candidates_of_previous_8u.setTo(0, this->_previous_depth_mask);

    this->_moving_pts_of_current_idx_depth->indices.clear();
    this->_moving_pts_of_previous_idx_depth->indices.clear();

    Image8u2Indices(this->_candidates_of_current_8u, this->_moving_pts_of_current_idx_depth);
    Image8u2Indices(this->_candidates_of_previous_8u, this->_moving_pts_of_previous_idx_depth);
}

void ShapeReconstruction::_TestMotionCoherencyOfPointsInImageLocationsWhereDepthChanged()
{
    // Estimate the transformations from the twists
    this->_EstimateTransformations();

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._TestMotionCoherencyOfPointsInImageLocationsWhereDepthChanged",
                          "[RB" << this->_rb_id << "]: Estimating which of the points that changed DEPTH moved coherently with the RB motion.");

    // Filter the previous point cloud, transform only the filtered points and find the nearest neighbors in the current point cloud
    this->_sqrt_dist.clear();
    this->_FindCandidatesInPreviousPC(this->_previous_ffs,
                                      this->_current_ffs,
                                      this->_moving_pts_of_previous_idx_depth,
                                      this->_current_to_previous_HTransform,
                                      this->_current_k_indices,
                                      this->_sqrt_dist);

    // Filter the current point cloud, transform only the filtered points and find the nearest neighbors in the previous point cloud
    this->_sqrt_dist.clear();
    this->_FindCandidatesInCurrentPC(this->_previous_ffs,
                                     this->_current_ffs,
                                     this->_moving_pts_of_current_idx_depth,
                                     this->_previous_to_current_HTransform,
                                     this->_previous_k_indices,
                                     this->_sqrt_dist);

    std::vector<int > invalid_indices;
    // Add the valid points of the current point cloud to the model
    for (unsigned int i = 0; i < _previous_k_indices.size(); i++)
    {
        if (_previous_k_indices[i].size() == 0)
        {
            invalid_indices.push_back(_moving_pts_of_current_idx_depth->indices.at(i));
        }
    }

    cv::Mat not_coherent_points = cv::Mat(IMG_HEIGHT, IMG_WIDTH, CV_8UC1);
    Indices2Image8u(invalid_indices, not_coherent_points);

    this->_acc_candidates_of_current.setTo(0, not_coherent_points);
}

//#define DISPLAY_IMAGES_DILWCC
void ShapeReconstruction::_DetectImageLocationsWhereColorChanges()
{
    // Create matrices
    cv::Mat current_hsv_image, current_h_channel, current_s_channel, current_v_channel;
    cv::Mat previous_hsv_image, previous_h_channel, previous_s_channel, previous_v_channel;
    cv::Mat difference_h_channel, circular_difference_h_channel, thresholded_difference_h_channel, difference_h_channel_wo_black, thresholded_difference_h_channel_wo_black;

    // Convert ROS msg to opencv
    cv_bridge::CvImagePtr previous_rgb_cv;
    previous_rgb_cv = cv_bridge::toCvCopy(this->_previous_ffs._rgb);

    cv_bridge::CvImagePtr current_rgb_cv;
    current_rgb_cv = cv_bridge::toCvCopy(this->_current_ffs._rgb);

#ifdef DISPLAY_IMAGES_DILWCC
    // Display original color images
    cv::imshow("color current", current_rgb_cv->image);
    cv::imshow("color previous", previous_rgb_cv->image);
#endif

    // Convert color to HSV images
    cv::cvtColor(previous_rgb_cv->image, previous_hsv_image, CV_BGR2HSV);
    cv::cvtColor(current_rgb_cv->image, current_hsv_image, CV_BGR2HSV);

    // Smooth images
    //cv::GaussianBlur(previous_hsv_image, previous_hsv_image,cv::Size(5,5), 0,0);
    //cv::GaussianBlur(current_hsv_image, current_hsv_image,cv::Size(5,5), 0,0);

    // Extract channels
    cv::extractChannel(current_hsv_image,current_h_channel,0);
    cv::extractChannel(current_hsv_image,current_s_channel,1);
    cv::extractChannel(current_hsv_image,current_v_channel,2);

    cv::extractChannel(previous_hsv_image,previous_h_channel,0);
    cv::extractChannel(previous_hsv_image,previous_s_channel,1);
    cv::extractChannel(previous_hsv_image,previous_v_channel,2);

    // Difference of hue -> HUE values are between 0 and 179 and represent the angle
    // of a cylindrical coordinates system. That implies that 180=0.
    cv::absdiff(current_h_channel,  previous_h_channel, difference_h_channel );
    circular_difference_h_channel = 180.0 - difference_h_channel;
    circular_difference_h_channel.copyTo(difference_h_channel, difference_h_channel >89);

#ifdef DISPLAY_IMAGES_DILWCC
    // Show difference of HUE
    cv::imshow("Circular difference of HUE", difference_h_channel);
#endif

    // We do not consider pixels that change between different black hue colors, because black identification if really bad
    difference_h_channel.copyTo(difference_h_channel_wo_black);
    //difference_h_channel_wo_black.setTo(cv::Scalar(0), current_s_channel < 90 & current_v_channel < 50 & previous_s_channel < 90 & previous_v_channel < 50);
    difference_h_channel_wo_black.setTo(cv::Scalar(0), current_s_channel < 90 | previous_s_channel < 90 );

#ifdef DISPLAY_IMAGES_DILWCC
    // Show pixels with low saturation or value
    cv::imshow("Low current_s_channel", current_s_channel < 90);
    //cv::imshow("Low current_v_channel", current_v_channel < 10);
    cv::imshow("Low previous_s_channel", previous_s_channel < 90);
    //cv::imshow("Low previous_v_channel", previous_v_channel < 10);
#endif

    // Threshold the circular hue difference to find points that change enough
    cv::threshold(difference_h_channel, thresholded_difference_h_channel, 20, 255, cv::THRESH_BINARY);

#ifdef DISPLAY_IMAGES_DILWCC
    cv::imshow("Thresholded circular difference of HUE", thresholded_difference_h_channel);
#endif

    // Threshold the circular hue difference without black pixels to find points that change enough
    cv::threshold(difference_h_channel_wo_black, thresholded_difference_h_channel_wo_black, 15, 255, cv::THRESH_BINARY);

#ifdef DISPLAY_IMAGES_DILWCC
    cv::imshow("Thresholded circular difference of HUE without black pixels", thresholded_difference_h_channel_wo_black);
#endif

    cv::Mat thresholded_difference_h_channel_wo_black_wo_noisy_pixels;
    int openning_size = 1;
    // Element: 0: Rect - 1: Cross - 2: Ellipse
    int openning_element_type = 2;
    cv::Mat openning_element = cv::getStructuringElement( openning_element_type, cv::Size( 2*openning_size + 1, 2*openning_size+1 ), cv::Point( openning_size, openning_size ) );
    cv::morphologyEx(thresholded_difference_h_channel_wo_black, thresholded_difference_h_channel_wo_black_wo_noisy_pixels, cv::MORPH_OPEN, openning_element);

#ifdef DISPLAY_IMAGES_DILWCC
    cv::imshow("Thresholded circular difference of HUE without black pixels nor NOISY pixels", thresholded_difference_h_channel_wo_black_wo_noisy_pixels);
#endif

    // We use color to detect things that move and do not change depth. Therefore, we set to zero
    // points that changed their color AND their depth
    cv::Mat changing_depth_points;
    if(this->_depth_filling)
    {
        cv::absdiff(this->_current_ffs._dm_filled, this->_previous_ffs._dm_filled, changing_depth_points);
    }else{
        cv::absdiff(this->_current_ffs._dm->image, this->_previous_ffs._dm->image, changing_depth_points);
    }

    // The mask of moving points in current frame are the points that changed color and that are valid
    cv::Mat motion_mask_current= thresholded_difference_h_channel_wo_black_wo_noisy_pixels.clone();
    motion_mask_current.setTo(cv::Scalar(0), this->_current_depth_mask);

    // The mask of moving points in previous frame are the points that changed color and that are valid
    cv::Mat motion_mask_previous = thresholded_difference_h_channel_wo_black_wo_noisy_pixels.clone();
    motion_mask_previous.setTo(cv::Scalar(0), this->_previous_depth_mask);

#ifdef DISPLAY_IMAGES_DILWCC
    cv::imshow("Current color candidates", motion_mask_current);
    cv::imshow("Previous color candidates", motion_mask_previous);
#endif

#ifdef DISPLAY_IMAGES_DILWCC
    cv::waitKey(-1);
#endif

    Image8u2Indices(motion_mask_current, this->_moving_pts_of_current_idx_color);
    Image8u2Indices(motion_mask_previous, this->_moving_pts_of_previous_idx_color);

    if(this->_record_videos)
    {
        _videos.write("original_color",ros::Time::now(), current_rgb_cv->toImageMsg());
        sensor_msgs::ImagePtr dm_msg;
        DepthImage2CvImage(thresholded_difference_h_channel, dm_msg);
        this->_videos.write("changes_in_color", this->_current_ffs._time, dm_msg);
    }
}

void ShapeReconstruction::_TestMotionCoherencyOfPointsInImageLocationsWhereColorChanged()
{
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._TestMotionCoherencyOfPointsInImageLocationsWhereColorChanged",
                          "[RB" << this->_rb_id << "]: Estimating which of the points that changed COLOR moved coherently with the RB motion.");

    // Filter the previous point cloud, transform only the filtered points and find the nearest neighbors in the current point cloud
    this->_FindCandidatesInPreviousPC(this->_previous_ffs,
                                      this->_current_ffs,
                                      this->_moving_pts_of_previous_idx_color,
                                      this->_current_to_previous_HTransform,
                                      this->_current_k_indices,
                                      this->_sqrt_dist);

    // Filter the current point cloud, transform only the filtered points and find the nearest neighbors in the previous point cloud
    this->_FindCandidatesInCurrentPC(this->_previous_ffs,
                                     this->_current_ffs,
                                     this->_moving_pts_of_current_idx_color,
                                     this->_previous_to_current_HTransform,
                                     this->_previous_k_indices,
                                     this->_sqrt_dist);
}

void ShapeReconstruction::_FindCandidatesInPreviousPC(FrameForSegmentation& _previous_ffs,
                                                      const FrameForSegmentation& _current_ffs,
                                                      pcl::PointIndices::Ptr& _moving_pts_of_previous_idx,
                                                      Eigen::Matrix4d& _current_to_previous_HTransform,
                                                      std::vector<std::vector<int > >& _previous_k_indices,
                                                      std::vector<std::vector<float > >& _previous_sqrt_dist)
{
    _previous_ffs._pc_moving_pts =  omip::SRPointCloud::Ptr(new omip::SRPointCloud);
    this->_extractor.setInputCloud(_previous_ffs._pc);
    this->_extractor.setIndices(_moving_pts_of_previous_idx);
    this->_extractor.setNegative(false);
    this->_extractor.filter(*_previous_ffs._pc_moving_pts);

    std::vector<std::vector<int > > previous_k_indices;
    if(_previous_ffs._pc_moving_pts->points.size())
    {
        pcl::transformPointCloud<SRPoint>(*_previous_ffs._pc_moving_pts, *_previous_ffs._pc_moving_pts_transf, _current_to_previous_HTransform.cast<float>());

        this->_knn->setInputCloud(_current_ffs._pc_without_nans);
        std::vector<int> empty;

        this->_knn->radiusSearch(*_previous_ffs._pc_moving_pts_transf, empty, this->_knn_min_radius, previous_k_indices, _previous_sqrt_dist);

        ROS_INFO_STREAM_NAMED("ShapeReconstruction._FindCandidatesInPreviousPC", "[RB" << this->_rb_id << "]: After radius search for finding candidates in previous PC. Queries: "
                              << _previous_ffs._pc_moving_pts_transf->points.size() );

    }

    _previous_k_indices.insert(_previous_k_indices.end(), previous_k_indices.begin(), previous_k_indices.end());
}

void ShapeReconstruction::_FindCandidatesInCurrentPC(const FrameForSegmentation& _previous_ffs,
                                                     FrameForSegmentation& _current_ffs,
                                                     pcl::PointIndices::Ptr& _moving_pts_of_current_idx,
                                                     Eigen::Matrix4d& _previous_to_current_HTransform,
                                                     std::vector<std::vector<int > >& _current_k_indices,
                                                     std::vector<std::vector<float > >& _current_sqrt_dist)
{
    _current_ffs._pc_moving_pts =  omip::SRPointCloud::Ptr(new omip::SRPointCloud);
    this->_extractor.setInputCloud(_current_ffs._pc);
    this->_extractor.setIndices(_moving_pts_of_current_idx);
    this->_extractor.setNegative(false);
    this->_extractor.filter(*_current_ffs._pc_moving_pts);

    std::vector<std::vector<int > > current_k_indices;
    if(_current_ffs._pc_moving_pts->points.size())
    {
        pcl::transformPointCloud<SRPoint>(*_current_ffs._pc_moving_pts, *_current_ffs._pc_moving_pts_transf, _previous_to_current_HTransform.cast<float>());

        this->_knn->setInputCloud(_previous_ffs._pc_without_nans);

        std::vector<int> empty;
        this->_knn->radiusSearch(*_current_ffs._pc_moving_pts_transf, empty, this->_knn_min_radius, current_k_indices, _current_sqrt_dist);

        ROS_INFO_STREAM_NAMED("ShapeReconstruction._FindCandidatesInPreviousPC", "[RB" << this->_rb_id << "]: After radius search for finding candidates in current PC. Queries: "
                              << _current_ffs._pc_moving_pts_transf->points.size() );
    }

    _current_k_indices.insert(_current_k_indices.end(), current_k_indices.begin(), current_k_indices.end());
}

void ShapeReconstruction::_MergeValidPointsIntoModels()
{
    // We create these transformed point clouds in the general function to avoid repeating it in the subfunctions

    // Transform the points of the current point cloud that belong to the model to the object frame (corresponds to the frame of the first point cloud)
    omip::SRPointCloud::Ptr points_of_current_in_origin(new omip::SRPointCloud);
    pcl::transformPointCloud<SRPoint>(*this->_current_ffs._pc_without_nans, *points_of_current_in_origin, this->_current_HTransform_inv.cast<float>());

    // Transform the points of the previous point cloud that belong to the model into the object frame (corresponds to the frame of the first point cloud)
    omip::SRPointCloud::Ptr points_of_previous_in_origin(new omip::SRPointCloud);
    pcl::transformPointCloud<SRPoint>(*this->_previous_ffs._pc_without_nans, *points_of_previous_in_origin, this->_previous_HTransform_inv.cast<float>());

    //Now we have all the indices together
    _MergeValidPointsIntoModels("DEPTHANDCOLOR",
                                    points_of_current_in_origin,
                                    points_of_previous_in_origin,
                                    this->_rb_shape,
                                    this->_current_k_indices,
                                    this->_previous_k_indices);

    this->_candidates->points.clear();
    this->_candidates->width = 0;
}

void ShapeReconstruction::_MergeValidPointsIntoModels(const std::string& logger_name,
                                                      omip::SRPointCloud::Ptr& points_of_current_in_origin,
                                                      omip::SRPointCloud::Ptr& points_of_previous_in_origin,
                                                      omip::SRPointCloud::Ptr& rb_model,
                                                      const std::vector<std::vector<int > >& current_k_indices,
                                                      const std::vector<std::vector<int > >& previous_k_indices)
{
    int model_size_before_adding = rb_model->width;

    // Add the valid points of the current point cloud to the model
    for (unsigned int i = 0; i < current_k_indices.size(); i++) {
        if (current_k_indices[i].size() > 0){
            rb_model->points.push_back(points_of_current_in_origin->points[current_k_indices[i].at(0)]);
            rb_model->width++;
        }
    }

    // Add the valid points of the previous point cloud to the model
    for (unsigned int i = 0; i < previous_k_indices.size(); i++) {
        if (previous_k_indices[i].size() > 0){
            rb_model->points.push_back(points_of_previous_in_origin->points[previous_k_indices[i].at(0)]);
            rb_model->width++;
        }
    }

    int number_of_points_added = rb_model->width - model_size_before_adding;
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._MergeValidPointsIntoModels", "[RB" << this->_rb_id << "] "
                          << ": Added " << number_of_points_added << " points to the model based on changes in " << logger_name);
}

void ShapeReconstruction::_RemoveInconsistentPointsFromModelsAndExtendToRegions()
{
    // clean up models by removing points inconsistent with the current view
    this->_RemoveInconsistentPointsFromRBModel("BeforeExtendingToRegions", this->_current_HTransform,
                                               this->_rb_shape,this->_current_ffs._dm->image,this->_current_ffs._pc,this->_rb_segment);

    if(_estimate_supervoxels)
    {
        // After cleaning up the models we extend to regions
        this->_ExtendPointsToRegions();

        this->_RemoveInconsistentPointsFromRBModel("AfterExtendingToRegions", this->_current_HTransform,
                                                   this->_rb_shape,this->_current_ffs._dm->image,this->_current_ffs._pc,this->_rb_segment);
    }
}

void ShapeReconstruction::_FilterModel()
{
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._FilterModel", "[RB" << this->_rb_id << "]: Before approximate voxel grid filter. Num of points: "
                           << this->_rb_shape->points.size() );
    this->_approximate_voxel_grid_filter.setInputCloud (this->_rb_shape);
    this->_approximate_voxel_grid_filter.filter (*this->_rb_shape);
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._FilterModel", "[RB" << this->_rb_id << "]: After approximate voxel grid filter. Num of points: "
                           << this->_rb_shape->points.size() );

    // Apply the outlier removal filter to keep the model clean of small noisy clusters
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._FilterModel", "[RB" << this->_rb_id << "] " << ": Before outlier removal. Num of points: "
                          << this->_rb_shape->points.size() );
    this->_radius_outlier_removal.setInputCloud(this->_rb_shape);
    this->_radius_outlier_removal.filter (*this->_rb_shape);
    ROS_INFO_STREAM_NAMED("ShapeReconstruction._FilterModel", "[RB" << this->_rb_id << "] " << ": After outlier removal. Num of points: "
                          << this->_rb_shape->points.size() );

}

void ShapeReconstruction::_RemoveInconsistentPointsFromRBModel(const std::string logger_name,
                                                               const Eigen::Matrix4d& HTransform,
                                                               SRPointCloud::Ptr& rb_shape,
                                                               cv::Mat& current_dm,
                                                               SRPointCloud::Ptr current_pc,
                                                               SRPointCloud::Ptr& rb_segment)
{
    // are self-occlusions handled?
    // yes they should be accounted for implicitly because if the "front" & the "back" are in the model, only
    // the front (which should be visible in current) will be projected to rb_rip.

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._RemoveInconsistentPointsFromRBModel", "[RB" << this->_rb_id << "] " << logger_name <<
                          ": Removing inconsistent points");

    // move the rb_shape into the current frame
    omip::SRPointCloud::Ptr rb_shape_current(new omip::SRPointCloud);
    pcl::transformPointCloud<SRPoint>(*rb_shape, *rb_shape_current, HTransform.cast<float>());

    // move the rb_shape into the current frame (new variant)
    omip::SRPointCloud::Ptr rb_shape_current_new(new omip::SRPointCloud);
    *rb_shape_current_new = *rb_shape_current;

    std::stringstream rb_id_str;
    rb_id_str << _rb_id;

    pcl::PointIndicesPtr indices_to_remove(new pcl::PointIndices);

    // these are the points in the current image that are consistent
    // with the model -> the actual segment in the image
    pcl::PointIndicesPtr indices_matching_in_model(new pcl::PointIndices);
    pcl::PointIndicesPtr indices_matching_in_current(new pcl::PointIndices);

    // find points that are inconsistent between moved rb_shape
    // and current depth image
    _FindInconsistentPoints(rb_shape_current_new, current_dm, // input
                               indices_to_remove, // output
                               indices_matching_in_model, // output
                               indices_matching_in_current // output
                               );

    // remove indices
    SRPointCloud::Ptr rb_new_clean(new SRPointCloud);
    if (_remove_inconsistent_points)
    {
        // move the rb_shape into the current frame
        this->_extractor.setNegative(true);
        this->_extractor.setInputCloud(rb_shape);
        this->_extractor.setIndices(indices_to_remove);
        this->_extractor.setKeepOrganized(false);
        this->_extractor.filter(*rb_new_clean);
    } else {
        indices_to_remove->indices.clear();
    }

    // create segment (always in current frame)
    SRPointCloud::Ptr rb_segment_new(new SRPointCloud);
    this->_extractor.setNegative(false);
    this->_extractor.setInputCloud(current_pc);
    this->_extractor.setIndices(indices_matching_in_current);
    this->_extractor.setKeepOrganized(true);
    this->_extractor.filter(*rb_segment_new);
    this->_extractor.setKeepOrganized(false);

    cv::Mat rb_segment_new_dm(IMG_HEIGHT, IMG_WIDTH, CV_32FC1);
    OrganizedPC2DepthMap(rb_segment_new, rb_segment_new_dm);

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._RemoveInconsistentPointsAndComputeSegment", "[RB" << this->_rb_id << "] " << logger_name << ":" << std::endl
                          << "\tPoints of the original model: " << rb_shape->points.size() << "," << std::endl
                          << "\tPoints matching in model: " << indices_matching_in_model->indices.size() << "," << std::endl
                          << "\tPoints matching in model: " << rb_new_clean->points.size()  << "," << std::endl
                          << "\tPoints matching in current point cloud: " << indices_matching_in_current->indices.size() << std::endl
                          << "\tPoints removed: " << indices_to_remove->indices.size() << (indices_to_remove->indices.empty() ? " (NO POINTS REMOVED) " : "")
                          );

    if (_remove_inconsistent_points)
    {
        rb_shape = rb_new_clean;
    }
    rb_segment = rb_segment_new;
}

void ShapeReconstruction::_FindInconsistentPoints(const omip::SRPointCloud::Ptr& pc_source,
                                                     const cv::Mat & dm_true,
                                                     pcl::PointIndicesPtr& indices_to_remove,
                                                     pcl::PointIndicesPtr& indices_matching_in_true,
                                                     pcl::PointIndicesPtr& indices_matching_in_dm,
                                                     const double min_depth_error) {

    indices_to_remove->indices.clear();

    using ::shape_reconstruction::RangeImagePlanar;
    RangeImagePlanar::Ptr dm_source_rip(new RangeImagePlanar);

    Eigen::Affine3f sensor_pose;
    sensor_pose.matrix() = Eigen::Matrix4f::Identity(); // this->_current_HTransform_inv.cast<float>();
    pcl::RangeImagePlanar::CoordinateFrame coordinate_frame = pcl::RangeImagePlanar::CAMERA_FRAME;

    int width = dm_true.cols, height = dm_true.rows;
    dm_source_rip->matchPointCloudAndImage (
                *pc_source,
                width,
                height,
                this->_ci.P[2], //width/2 -0.5, //319.5, // 329.245223575443,
            this->_ci.P[6], //height/2 - 0.5, // 239.5, //  239.458
            this->_ci.P[0], // fx
            this->_ci.P[5], // fy
            sensor_pose,
            coordinate_frame,
            dm_true,
            min_depth_error,
            indices_matching_in_true,
            indices_matching_in_dm,
            indices_to_remove
            );

}

void ShapeReconstruction::growSVRecursively(uint32_t label_sv_seed,
                                            std::vector<uint32_t>& labels_extension,
                                            std::multimap<uint32_t, uint32_t>& supervoxel_adjacency,
                                            std::vector<uint32_t>& sv_containing_supporting_features,
                                            int& distance_to_feature_sv,
                                            std::map<uint32_t, std::unordered_set<int> >& labels_of_segments_to_extend)
{
    std::pair< std::multimap<uint32_t, uint32_t>::iterator, std::multimap<uint32_t, uint32_t>::iterator > neighbors;

    // Get the adjacency map of the sv
    neighbors =supervoxel_adjacency.equal_range(label_sv_seed);

    // Iterate over the pairs of sv-neighbor
    for(std::multimap<uint32_t, uint32_t>::iterator neighbor_it = neighbors.first; neighbor_it!= neighbors.second; ++neighbor_it)
    {
        bool add_neighbor = false;
        // Check if the neighbor sv was already added
        if(std::find(labels_extension.begin(), labels_extension.end(), neighbor_it->second)  != labels_extension.end())
        {
            add_neighbor = false;
        }else{
            // Check if the neighbor sv should be added
            // if it contains a feature OR if (it contains enough points that changed (color OR depth) AND the distance to a feature-containing SV is small)
            if(std::find(sv_containing_supporting_features.begin(), sv_containing_supporting_features.end(), neighbor_it->second)  != sv_containing_supporting_features.end())
            {
                add_neighbor = true;
                distance_to_feature_sv = 0;
            }else{
                std::map<uint32_t, std::unordered_set<int> >::iterator set_it = labels_of_segments_to_extend.find(neighbor_it->second);
                if(set_it != labels_of_segments_to_extend.end())
                {
                    if(set_it->second.size() > this->_min_number_model_pixels_in_sv)
                    {
                        if(distance_to_feature_sv <= 2)
                        {
                            add_neighbor = true;
                            distance_to_feature_sv++;
                        }
                    }
                }

            }
        }

        if(add_neighbor)
        {
            labels_extension.push_back(neighbor_it->second);
            growSVRecursively(neighbor_it->second,
                              labels_extension,
                              supervoxel_adjacency,
                              sv_containing_supporting_features,
                              distance_to_feature_sv,
                              labels_of_segments_to_extend);
        }
    }
}

void ShapeReconstruction::_ExtendPointsToRegions()
{
    std::vector<uint32_t> labels_to_extend;

    //Retrieve the labeled point cloud. Labels == Supervoxel ids
    pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud = (*_supervoxelizer_ptr_ptr)->getLabeledCloud();

    //Remove NaNs from the labeled point cloud
    pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud_not_nans(new pcl::PointCloud<pcl::PointXYZL>());
    std::vector<int> unused;
    pcl::removeNaNFromPointCloud< pcl::PointXYZL >(*labeled_cloud, *labeled_cloud_not_nans, unused);

    //Copy labeled point cloud without NaNs to a point cloud of XYZRGB points
    omip::SRPointCloud::Ptr labeled_cloud_not_nans_xyzrgb(new omip::SRPointCloud);
    pcl::copyPointCloud(*labeled_cloud_not_nans, *labeled_cloud_not_nans_xyzrgb);

    //Pass the new point cloud as input to a KNN. We will use it to find neighbors of the features and the points of the model
    this->_knn->setInputCloud(labeled_cloud_not_nans_xyzrgb);

    //Copy the supporting features of this RB into a point cloud without labels (only xyz)
    pcl::PointCloud<pcl::PointXYZ>::Ptr supporting_feats_no_labels(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*this->_supporting_features, *supporting_feats_no_labels);

    //Find the neighbor of each supporting feature in the point cloud of supervoxels -> find the supervoxel that contains each supporting feature
    //and add the supervoxels to the candidates
    for(int idx_sf =0; idx_sf<supporting_feats_no_labels->points.size(); idx_sf++)
    {
        std::vector<int> neighbors;
        std::vector<float> distances;
        this->_knn->nearestKSearchT<pcl::PointXYZ>(supporting_feats_no_labels->points[idx_sf], 1, neighbors, distances);
        if(std::find(labels_to_extend.begin(), labels_to_extend.end(), labeled_cloud_not_nans->points[neighbors[0]].label) == labels_to_extend.end())
        {
            if(labeled_cloud_not_nans->points[neighbors[0]].label != 0 )
            {
                labels_to_extend.push_back(labeled_cloud_not_nans->points[neighbors[0]].label);
            }
        }
    }

    // THIS PART COULD BE IMPROVED TO ACCELERATE THE PROCESS!
    // Move the model to the current location
    // and find neighbors in the labeled point cloud (supervoxels)
    omip::SRPointCloud::Ptr model_in_current_frame(new omip::SRPointCloud);
    std::vector<std::vector<int > > indices_model;
    std::vector<std::vector<float > > unused2;
    unused.clear();
    if(this->_rb_shape->points.size())
    {
        pcl::transformPointCloud<SRPoint>(*this->_rb_shape, *model_in_current_frame, this->_current_HTransform.cast<float>());
        this->_knn->radiusSearch(*model_in_current_frame, unused, this->_knn_min_radius, indices_model, unused2);
    }

    // Build a set that contains all the labels of the supervoxels containing points of the depth-based
    // and the indices of the points of the supervoxels that are neighbors of the models (this avoids counting the same point several times)
    std::map<uint32_t, std::unordered_set<int> > labels_of_segments_to_extend;
    for (unsigned int i = 0; i < indices_model.size(); i++)
    {
        for(unsigned int k=0; k < indices_model[i].size(); k++)
        {
            if(indices_model[i].at(k))
            {
                // For the first point we need to create the entry of the map
                if(labels_of_segments_to_extend.find(labeled_cloud_not_nans->points.at(indices_model[i].at(k)).label) ==  labels_of_segments_to_extend.end())
                {
                    std::unordered_set<int> initial_set_of_indices = {indices_model[i].at(k)};
                    labels_of_segments_to_extend[labeled_cloud_not_nans->points.at(indices_model[i].at(k)).label] = initial_set_of_indices;
                }
                else{
                    // If this point was not counted before as neighbor of another point we add its label
                    if(labels_of_segments_to_extend.at(labeled_cloud_not_nans->points.at(indices_model[i].at(k)).label).find(indices_model[i].at(k))
                            == labels_of_segments_to_extend.at(labeled_cloud_not_nans->points.at(indices_model[i].at(k)).label).end())
                    {
                        labels_of_segments_to_extend.at(labeled_cloud_not_nans->points.at(indices_model[i].at(k)).label).insert(indices_model[i].at(k));
                    }
                }
            }
        }
    }

    std::map<uint32_t, std::unordered_set<int> >::iterator it = labels_of_segments_to_extend.begin();
    std::map<uint32_t, std::unordered_set<int> >::iterator itend = labels_of_segments_to_extend.end();
    // Iterate over the map-counter and add the supervoxels that contain enough points
    for(; it != itend; it++)
    {
        if(it->second.size() > _min_number_model_pixels_in_sv && std::find(labels_to_extend.begin(), labels_to_extend.end(), it->first) == labels_to_extend.end())
        {
            if(it->first != 0)
            {
                labels_to_extend.push_back(it->first);
            }
        }
    }
    // END OF (THIS PART COULD BE IMPROVED TO ACCELERATE THE PROCESS!)

    std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
    (*_supervoxelizer_ptr_ptr)->getSupervoxelAdjacency (supervoxel_adjacency);

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._ExtendPointsToRegions",
                          "[RB" << this->_rb_id << "]: Superpixels to extend the results: " << labels_to_extend.size());

    if(this->_extend_to_neighbor_sv)
    {
        this->_ExtendToNeighborSV(supervoxel_adjacency, labels_to_extend);

        ROS_INFO_STREAM_NAMED("ShapeReconstruction._ExtendPointsToRegions",
                              "[RB" << this->_rb_id << "]: Superpixels to extend the results (after extending to SV of similar properties): " << labels_to_extend.size());
    }

    // Add the points of the supervoxels to extend
    shape_reconstruction::PassThrough<pcl::PointXYZL>::Ptr pt_filter(new shape_reconstruction::PassThrough<pcl::PointXYZL>());
    pt_filter->setInputCloud(boost::shared_ptr<pcl::PointCloud<pcl::PointXYZL> >(labeled_cloud_not_nans));
    pt_filter->setFilterFieldName("label");
    omip::SRPointCloud::Ptr points_of_current_in_origin(new omip::SRPointCloud);
    pcl::transformPointCloud<SRPoint>(*this->_current_ffs._pc_without_nans, *points_of_current_in_origin, this->_current_HTransform_inv.cast<float>());

    pt_filter->setLabels(labels_to_extend);

    boost::shared_ptr<std::vector<int> > passing_indices_ptr = boost::shared_ptr<std::vector<int> >(new std::vector<int>());
    pt_filter->filter(*passing_indices_ptr);

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._ExtendPointsToRegions",
                           "[RB" << this->_rb_id << "]: Adding " << (*passing_indices_ptr).size() << " points to the extended model (currently " <<this->_rb_shape->width << " points)");
    for (unsigned int i = 0; i < (*passing_indices_ptr).size(); i++)
    {
        this->_rb_shape->points.push_back(points_of_current_in_origin->points[(*passing_indices_ptr)[i]]);
        this->_rb_shape->width++;
    }
}

void ShapeReconstruction::_ExtendToNeighborSV(std::multimap<uint32_t, uint32_t>& supervoxel_adjacency,
                                              std::vector<uint32_t>& labels_extension)
{
    //EXPERIMENTAL: Naive extension of the SV to neighboring SV with similar properties

    int number_of_sv = _supervoxel_clusters_ptr->size();
    std::vector<uint32_t> labels_extension_safety_copy = labels_extension;

    //We retrieve the range of elements with each of the added SV
    std::pair< std::multimap<uint32_t, uint32_t>::iterator, std::multimap<uint32_t, uint32_t>::iterator > neighbors;

    int num_extended_labels = labels_extension.size();
    int num_extended_labels_with_neigbors = 0;

    // This repeats until we do not add any other label
    while(num_extended_labels != num_extended_labels_with_neigbors)
    {
        num_extended_labels = labels_extension.size();
        std::vector<uint32_t> labels_extension_copy = labels_extension;
        for(std::vector<uint32_t>::iterator labels_it = labels_extension.begin(); labels_it!=labels_extension.end(); labels_it++)
        {
            // Find the neighbors of the sv
            neighbors =supervoxel_adjacency.equal_range(*labels_it);

            // Query the supervoxel to be extended
            if(_supervoxel_clusters_ptr->find(*labels_it) != _supervoxel_clusters_ptr->end())
            {
                pcl::Supervoxel<pcl::PointXYZRGB>::Ptr sv = _supervoxel_clusters_ptr->at(*labels_it);

                // Normal of the supervoxel to be extended
                Eigen::Vector3d sv_normal(sv->normal_.data_n[0], sv->normal_.data_n[1], sv->normal_.data_n[2]);

                // Position and color (RGB) of the supervoxel to be extended
                pcl::PointXYZRGB sv_centroid_xyzrgb( sv->centroid_.r, sv->centroid_.g, sv->centroid_.b);
                sv_centroid_xyzrgb.x = sv->centroid_.x;
                sv_centroid_xyzrgb.y = sv->centroid_.y;
                sv_centroid_xyzrgb.z = sv->centroid_.z;

                // Position and color (HSV) of the supervoxel to be extended
                pcl::PointXYZHSV sv_centroid_xyzhsv;
                pcl::PointXYZRGBtoXYZHSV(sv_centroid_xyzrgb, sv_centroid_xyzhsv);

                // Iterate over the pairs of sv-neighbor
                for(std::multimap<uint32_t, uint32_t>::iterator neighbor_it = neighbors.first; neighbor_it!= neighbors.second; ++neighbor_it)
                {
                    // Check if the neighbor was already added
                    if(std::find(labels_extension_copy.begin(), labels_extension_copy.end(), neighbor_it->second)  == labels_extension_copy.end() )
                    {
                        // Query the neighbor supervoxel to extend to
                        pcl::Supervoxel<pcl::PointXYZRGB>::Ptr nsv = _supervoxel_clusters_ptr->at(neighbor_it->second);

                        // Normal of the neighbor supervoxel to extend to
                        Eigen::Vector3d nsv_normal(nsv->normal_.data_n[0], nsv->normal_.data_n[1], nsv->normal_.data_n[2]);

                        // Position and color (RGB) of the neighbor supervoxel to extend to
                        pcl::PointXYZRGB nsv_centroid_xyzrgb( nsv->centroid_.r, nsv->centroid_.g, nsv->centroid_.b);
                        nsv_centroid_xyzrgb.x = nsv->centroid_.x;
                        nsv_centroid_xyzrgb.y = nsv->centroid_.y;
                        nsv_centroid_xyzrgb.z = nsv->centroid_.z;

                        // Position and color (HSV) of the neighbor supervoxel to extend to
                        pcl::PointXYZHSV nsv_centroid_xyzhsv;
                        pcl::PointXYZRGBtoXYZHSV(nsv_centroid_xyzrgb, nsv_centroid_xyzhsv);

                        // We compare the mean normal and the color of the centroid point
                        // CAREFUL: h values in pcl are between 0 and 360
                        double h_diff = std::fabs(sv_centroid_xyzhsv.h - nsv_centroid_xyzhsv.h);
                        h_diff = std::fmod(h_diff, 360.0);
                        h_diff = h_diff > 180.0 ? 360.0 - h_diff : h_diff;
                        // TODO: Handle black and white regions (use s and v)

                        if(std::fabs(sv_normal.dot(nsv_normal) - 1.0) < _similarity_in_normal*(M_PI/180.0) && h_diff < _similarity_in_h)
                        {
                            labels_extension_copy.push_back(neighbor_it->second);
                        }
                    }
                }
            }
        }
        num_extended_labels_with_neigbors = labels_extension_copy.size();
        labels_extension = labels_extension_copy;
    }

    // Emergency brake! -> We are adding too many svs
    int added_supervoxels = labels_extension.size() - labels_extension_safety_copy.size();
    // Options: max number of sv to add (absolute) or max fraction of the total amount of sv to add (relative to the total amount of sv)
    if((double)added_supervoxels/(double)number_of_sv > 0.2 )
    {
        ROS_INFO_STREAM_NAMED("ShapeReconstruction._ExtendPointsToRegions","We are adding too many supervoxels! Undo!");
        labels_extension = labels_extension_safety_copy;
    }
}

void ShapeReconstruction::_EstimateTransformations()
{
    ROSTwist2EigenTwist(this->_previous_ffs._transformation.twist, this->_previous_twist);
    Twist2TransformMatrix(this->_previous_twist, this->_previous_HTransform);
    this->_previous_HTransform_inv = this->_previous_HTransform.inverse();

    ROSTwist2EigenTwist(this->_current_ffs._transformation.twist, this->_current_twist);
    Twist2TransformMatrix(this->_current_twist, this->_current_HTransform);
    this->_current_HTransform_inv = this->_current_HTransform.inverse();

    ROS_DEBUG_STREAM_NAMED("ShapeReconstruction._EstimateTransformations",  "[RB" << this->_rb_id << "]: _current_HTransform " << std::endl <<
                           _current_HTransform);

    this->_current_to_previous_HTransform = this->_current_HTransform*this->_previous_HTransform_inv;

    this->_previous_to_current_HTransform = this->_current_to_previous_HTransform.inverse();

    ROS_DEBUG_STREAM_NAMED("ShapeReconstruction._EstimateTransformations",  "[RB" << this->_rb_id << "]: current to previous " << std::endl <<
                           _current_to_previous_HTransform);
}

void ShapeReconstruction::_GenerateMesh(const omip::SRPointCloud::Ptr& pc_source,
                                        std::string shape_file_prefix)
{
    if(pc_source->points.size())
    {
        // Normal estimation*
        this->_rb_tree_for_normal_estimation_ptr->setInputCloud(pc_source);
        this->_rb_normal_estimator_ptr->setInputCloud(pc_source);
        this->_rb_normal_estimator_ptr->setSearchMethod (this->_rb_tree_for_normal_estimation_ptr);
        this->_rb_normal_estimator_ptr->compute(*this->_rb_estimated_normals_pc_ptr);

        // Concatenate the XYZ and normal fields*
        pcl::concatenateFields (*pc_source, *this->_rb_estimated_normals_pc_ptr, *this->_rb_position_color_and_normals_pc_ptr);
        this->_rb_tree_for_triangulation_ptr->setInputCloud (this->_rb_position_color_and_normals_pc_ptr);

        // Get result
        this->_rb_greedy_projection_triangulator_ptr->setInputCloud(this->_rb_position_color_and_normals_pc_ptr);
        this->_rb_greedy_projection_triangulator_ptr->setSearchMethod(this->_rb_tree_for_triangulation_ptr);
        this->_rb_greedy_projection_triangulator_ptr->reconstruct(*this->_rb_triangulated_mesh_ptr);

        // Additional vertex information
        //std::vector<int> parts = gp3.getPartIDs();
        //std::vector<int> states = gp3.getPointStates();

        std::string mesh_path = ros::package::getPath("shape_reconstruction") + std::string("/meshes/");
        std::stringstream rb_name_ss;
        rb_name_ss << shape_file_prefix << this->_rb_id << ".stl";

        std::string rb_mesh_full_file_name = mesh_path + rb_name_ss.str();

        pcl::io::mesh2vtk(*this->_rb_triangulated_mesh_ptr, this->_rb_polygon_data_ptr);

        this->_rb_polygon_writer_ptr->SetInputData (this->_rb_polygon_data_ptr);
        this->_rb_polygon_writer_ptr->SetFileName (rb_mesh_full_file_name.c_str ());
        this->_rb_polygon_writer_ptr->Write ();

        ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh",  "[RB" << this->_rb_id << "]: Resulting triangular mesh written to " << rb_mesh_full_file_name);
    }else{
        ROS_WARN_STREAM_NAMED("ShapeReconstruction.generateMesh",  "[RB" << this->_rb_id << "]: Impossible to generate a triangular mesh for this model, it doesn't contain any points!");
    }
}

void ShapeReconstruction::generateMesh()
{
    ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh",  "[RB" << this->_rb_id << "]: Called the generation of 3D triangular mesh from the model of RB" << this->_rb_id <<
                          " based on extending other models with supervoxels.");
    std::string shape_ext_d_and_c_file_prefix("shape_ext_d_and_c_rb");
    this->_GenerateMesh(this->_rb_shape, shape_ext_d_and_c_file_prefix);

    ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh",  "[RB" << this->_rb_id << "]: Finished the generation of 3D triangular mesh from the point cloud of RB " << this->_rb_id);
}

void ShapeReconstruction::getShapeModel(omip_msgs::ShapeModelsPtr shapes)
{
    omip_msgs::ShapeModel shape;
    shape.rb_id = this->_rb_id;
    sensor_msgs::PointCloud2 rb_shape_ros;
    rb_shape_ros.header.frame_id = "/camera_rgb_optical_frame";

    this->_rb_shape->header = this->_current_ffs._pc->header;
    pcl::toROSMsg(*this->_rb_shape, rb_shape_ros);

    shape.rb_shape_model = rb_shape_ros;
    shapes->rb_shape_models.push_back(shape);
}

void ShapeReconstruction::PublishMovedModelAndSegment(const ros::Time current_time,
                                                      const geometry_msgs::TwistWithCovariance &rb_transformation,
                                                      rosbag::Bag& bag,
                                                      bool bagOpen)
{
    Eigen::Twistd current_twist;
    ROSTwist2EigenTwist(rb_transformation.twist, current_twist);

    Eigen::Matrix4d current_HTransform;
    Twist2TransformMatrix(current_twist, current_HTransform);

    ROS_INFO_STREAM_NAMED("ShapeReconstruction._PublishMovedModel",  "[RB" << this->_rb_id << "]: Publishing " << this->_rb_shape->points.size() << " points reconstructed from RB" << this->_rb_id
                          << " based on extending both depth and color model with supervoxels.");
    this->_rb_shape->header = this->_current_ffs._pc->header;
    this->_rb_shape->header.frame_id = "/camera_rgb_optical_frame";
    this->_rb_shape_in_current_frame.reset(new omip::SRPointCloud);
    pcl::transformPointCloud<SRPoint>(*this->_rb_shape, *this->_rb_shape_in_current_frame, current_HTransform.cast<float>());
    sensor_msgs::PointCloud2 rb_shape_ext_d_and_c_in_current_frame_ros;
    pcl::toROSMsg(*this->_rb_shape_in_current_frame, rb_shape_ext_d_and_c_in_current_frame_ros);
    rb_shape_ext_d_and_c_in_current_frame_ros.header.frame_id = "/camera_rgb_optical_frame";
    this->_rb_shape_pub.publish(rb_shape_ext_d_and_c_in_current_frame_ros);

    if (bagOpen){
        bag.write(_rb_shape_pub.getTopic(), current_time, rb_shape_ext_d_and_c_in_current_frame_ros);
    }

    // segment
    {
        this->_rb_segment->header = this->_current_ffs._pc->header;
        this->_rb_segment->header.frame_id = "/camera_rgb_optical_frame";
        sensor_msgs::PointCloud2 rb_segment_in_current_frame_ros;
        pcl::toROSMsg(*this->_rb_segment, rb_segment_in_current_frame_ros);
        rb_segment_in_current_frame_ros.header.frame_id = "/camera_rgb_optical_frame";
        this->_rb_segment_pub.publish(rb_segment_in_current_frame_ros);
        if (bagOpen){
            bag.write(_rb_segment_pub.getTopic(), current_time, rb_segment_in_current_frame_ros);
        }
    }
}

void ShapeReconstruction::RemoveInconsistentPoints(const SRPointCloud::Ptr &pc_msg,
                                                   const geometry_msgs::TwistWithCovariance &rb_transformation)
{
    Eigen::Twistd current_twist;
    ROSTwist2EigenTwist(rb_transformation.twist, current_twist);
    Eigen::Matrix4d current_HTransform;
    Twist2TransformMatrix(current_twist, current_HTransform);

    SRPointCloud::Ptr current_pc(new SRPointCloud());
    pcl::copyPointCloud(*pc_msg, *current_pc);

    // Create a depth map from the current organized depth map
    cv::Mat current_dm;
    this->_current_ffs._dm->image.copyTo(current_dm);   // Just to initialize current_dm correctly
    OrganizedPC2DepthMap(current_pc, current_dm);

    this->_RemoveInconsistentPointsFromRBModel("", current_HTransform, this->_rb_shape, current_dm,current_pc, this->_rb_segment);

}

max_stars_repo_name: tu-rbo/omip

max_issues_repo_name: tu-rbo/omip

max_forks_repo_name: tu-rbo/omip


text:
#include <complex>
#include <memory>
#include <fftw3.h>
#include <armadillo>
using namespace std;
using namespace arma;

#ifndef __DEFT_HPP__
#define __DEFT_HPP__

class deft{

public:

    // constructors
    deft(const size_t, const size_t, const size_t, const double*, const double*, const double*);
    deft(const deft&);

    // read data
    double at(const size_t, const size_t, const size_t) const;
    double operator()(const size_t, const size_t, const size_t) const;

    // copy data
    void copy_data_from(const double*);

    // assignment
    void equals(const double);
    void operator=(const double);
    void equals(const deft);
    void operator=(const deft);

    // addition
    void addEquals(const double);
    void operator+=(const double);
    void addEquals(const deft);
    void operator+=(const deft);

    // subtraction
    void subtractEquals(const double);
    void operator-=(const double);
    void subtractEquals(const deft);
    void operator-=(const deft);
    // multiplication
    void multiplyEquals(const double);
    void operator*=(const double);
    void multiplyEquals(const deft);
    void operator*=(const deft);

    // division
    void divideEquals(const double);
    void operator/=(const double);
    void divideEquals(const deft);
    void operator/=(const deft);

    // elementwise math
    void pow(const double);

    // fourier transform operations
    void computeFT();
    void computeIFT();

    // derivatives (using fourier transforms)
    void compute_gradient_x();
    void compute_gradient_y();
    void compute_gradient_z();
    void compute_gradient_squared();
    void compute_laplacian();

    // integrate
    double integrate() const;

    // update cell geometry and reciprocal lattice vectors
    void updateGeometry(const double*, const double*, const double*);

    // read cell information
    double cellVecX(const size_t) const;
    double cellVecY(const size_t) const;
    double cellVecZ(const size_t) const;
    double cellLenX() const;
    double cellLenY() const;
    double cellLenZ() const;
    double vol() const;
    double dv() const;
    double kVecX(const size_t) const;
    double kVecX(const size_t, const size_t, const size_t) const;
    double kVecY(const size_t) const;
    double kVecY(const size_t, const size_t, const size_t) const;
    double kVecZ(const size_t) const;
    double kVecZ(const size_t, const size_t, const size_t) const;
    double kVecLen(const size_t) const;
    double kVecLen(const size_t, const size_t, const size_t) const;

    // interpolate
    deft* interpolate(const size_t new_x, const size_t new_y, const size_t new_z);

    // sum a function over a lattice
    void sum_over_lattice(mat loc, double (*func)(double));

private:
public:

    // real-space dimensions and data
    const size_t _xDim;
    const size_t _yDim;
    const size_t _zDim;
    const size_t _dimXYZ;
    // TODO: decide whether this should remain a pointer
    cube* _data;

private:

    // cell geometry
    shared_ptr<vec> _cellVecX;
    shared_ptr<vec> _cellVecY;
    shared_ptr<vec> _cellVecZ;
    shared_ptr<double> _cellLenX;
    shared_ptr<double> _cellLenY;
    shared_ptr<double> _cellLenZ;
    shared_ptr<double> _vol;
    shared_ptr<double> _dv;

public:

    // fourier-space dimensions and data
    const size_t _xDimFT;
    const size_t _yDimFT;
    const size_t _zDimFT;
    // TODO: add ft_numXYZ
    cx_cube* _dataFT;

private:

    // reciprocal lattice vectors
    shared_ptr<cube> _kVecX;
    shared_ptr<cube> _kVecY;
    shared_ptr<cube> _kVecZ;
    shared_ptr<cube> _kVecLen;

    // fftw info
    fftw_plan _planR2C;
    fftw_plan _planC2R;

};

#endif  //  __DEFT_HPP__

ext: hpp

max_stars_repo_name: EACcodes/deft

max_issues_repo_name: EACcodes/deft

max_forks_repo_name: EACcodes/deft


text:
//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_SDK_OPTION_OPTION_SPEC_HPP_INCLUDED
#define NT2_SDK_OPTION_OPTION_SPEC_HPP_INCLUDED

#include <nt2/sdk/option/option_term.hpp>
#include <boost/proto/core.hpp>

namespace nt2 { namespace details
{
  struct option_spec
      : boost::proto::when<
          boost::proto::assign<
              option_term
            , boost::proto::terminal<boost::proto::_>
          >
        , boost::proto::_value(boost::proto::_right)
      >
  {};
} }

#endif


text:
/*
    Copyright 2005-2007 Adobe Systems Incorporated

    Use, modification and distribution are subject to the Boost Software License,
    Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt).

    See http://opensource.adobe.com/gil for most recent version including documentation.
*/

/*************************************************************************************************/

#ifndef GIL_GRAY_H
#define GIL_GRAY_H

////////////////////////////////////////////////////////////////////////////////////////
/// \file
/// \brief Support for grayscale color space and variants
/// \author Lubomir Bourdev and Hailin Jin \n
///         Adobe Systems Incorporated
/// \date 2005-2007 \n Last updated on March 8, 2006
////////////////////////////////////////////////////////////////////////////////////////

#include "gil_config.hpp"
#include "utilities.hpp"
#include <boost/type_traits.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector_c.hpp>

namespace boost { namespace gil {

/// \ingroup ColorNameModel
/// \brief Gray
struct gray_color_t {};

/// \ingroup ColorSpaceModel
typedef mpl::vector1<gray_color_t> gray_t;

/// \ingroup LayoutModel
typedef layout<gray_t> gray_layout_t;

} }  // namespace boost::gil

#endif

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
//  (C) Copyright 2014 Alvaro J. Genial (http://alva.ro)
//  Use, modification and distribution are subject to the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt).

#ifndef AJG_SYNTH_ENGINES_DJANGO_VALUE_HPP_INCLUDED
#define AJG_SYNTH_ENGINES_DJANGO_VALUE_HPP_INCLUDED

#include <vector>
#include <utility>
#include <algorithm>

#include <boost/bind.hpp>
#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>

#include <ajg/synth/exceptions.hpp>
#include <ajg/synth/value_facade.hpp>
#include <ajg/synth/adapters/numeric.hpp>
#include <ajg/synth/detail/text.hpp>

namespace ajg {
namespace synth {
namespace engines {
namespace django {

template <class Traits>
struct value : value_facade<Traits, value> {
  public:

    typedef value                                                               value_type;
    typedef Traits                                                              traits_type;
    typedef typename value_type::facade_type                                    facade_type;

    typedef typename traits_type::boolean_type                                  boolean_type;
    typedef typename traits_type::size_type                                     size_type;
    typedef typename traits_type::floating_type                                 floating_type;
    typedef typename traits_type::datetime_type                                 datetime_type;
    typedef typename traits_type::string_type                                   string_type;

    typedef typename value_type::const_iterator                                 const_iterator;

    typedef std::vector<value_type>                                             sequence_type; // TODO: Derive from behavior_type.
    typedef std::map<string_type, value_type>                                   mapping_type; // TODO: Derive from behavior_type.

  private:

    typedef detail::text<string_type>                                           text;

  public:

    value() : facade_type(), safe_(false), token_(boost::none) {}
    value(value const& that) : facade_type(that), safe_(that.safe_), token_(that.token_) {} // TODO[c++11] = default.
    template <class T> value(T const& t) : facade_type(t), safe_(false), token_(boost::none) {}

  public:

    inline value_type copy() const { return *this; }

    inline value_type&  safe(boolean_type const safe) { return (this->safe_ = safe), *this; }
    inline boolean_type safe() const { return this->safe_; }

    inline value_type& mark_unsafe() { return this->safe(false), *this; }
    inline value_type& mark_safe() { return this->safe(true), *this; }

    inline value_type&        token(string_type const& token) { return (this->token_ = token), *this; }
    inline string_type const& token() const { BOOST_ASSERT(this->token_); return *this->token_; }

    inline boolean_type is_literal() const { return boolean_type(this->token_); }

    value_type escape() const {
        // XXX: Should this method escape binary and control characters?
        return text::escape_entities(this->to_string());
    }

    /*
    const_iterator find_attribute(value_type const& attribute) const {
        try {
            // First try to find the value itself.
            return this->find(attribute);
        }
        catch (invalid_method const& method) {
            if (method.name == "find") {
                try {
                    // If that fails, try using the value as an index.
                    return this->at(attribute.to_floating());
                }
                catch (std::exception const&) {
                    // Do nothing, and pass through to the `throw' below,
                    // so that we surface the original `find' failure.
                }
            }

            throw;
        }
    }
    */

    // TODO: Move this to traits.
    optional<value_type> get_attribute(value_type const& attribute) const {
        try {
            // First try to locate the value as a key.
            return this->index(attribute);
        }
        catch (invalid_method const& method) {
            if (method.name == "index") {
                try {
                    // If that fails, try using the value as an index.
                    const_iterator const it = this->at(attribute.to_floating());
                    return it == this->end() ? boost::none : *it;
                }
                catch (std::exception const&) {
                    // Do nothing, and pass through to the `throw' below,
                    // so that we surface the original `index' failure.
                }
            }

            throw;
        }
    }

    value_type must_get_attribute(value_type const& attribute) const {
        if (optional<value_type> attr = this->get_attribute(attribute)) {
            return *attr;
        }
        else {
            AJG_SYNTH_THROW(missing_attribute(text::narrow(attribute.to_string())));
        }
    }

    value_type must_get_trail(sequence_type const& trail) const {
        value_type value = *this;

        BOOST_FOREACH(value_type const& attribute, trail) {
            value = value.must_get_attribute(attribute);
        }

        return value;
    }

    typedef std::pair<value_type, sequence_type> group_type;
    typedef std::vector<group_type>              groups_type;

    groups_type group_by(value_type const& attrs) const {
        groups_type groups;
        value_type current_key;
        size_type i = 0;

        BOOST_FOREACH(value_type const& value, *this) {
            value_type const& key = value.must_get_trail(make_trail(attrs));

            // New group (either it's the first one or it has a different key.)
            if (!i++ || current_key != key) {
                current_key = key;
                groups.push_back(group_type(key, sequence_type(1, value)));
            }
            // Add to the current group.
            else {
                groups.back().second.push_back(value);
            }
        }

        return groups;
    }

    sequence_type to_sequence() const {
        sequence_type sequence;

        sequence.resize(this->size());
        size_type i = 0;
        BOOST_FOREACH(value_type const& value, *this) {
            sequence[i++] = value;
        }

        return sequence;
    }

    // NOTE: Returns a copy for now.
    value_type reverse() const {
        // TODO: Avoid copying the sequence for values with adapters that natively support rbegin/rend.
        sequence_type result;

        result.resize(this->size());
        size_type i = this->size() - 1;
        BOOST_FOREACH(value_type const& value, *this) {
            result[i--] = value;
        }

        return result;
    }

    // NOTE: Returns a copy for now.
    value_type sort_by(value_type const& attrs, boolean_type const reverse) const {
        // TODO: Avoid copying the sequence for values with adapters that natively support rbegin/rend.
        sequence_type result, trail = make_trail(attrs);

        result.reserve(this->size());
        BOOST_FOREACH(value_type const& value, *this) {
            result.push_back(value);
        }

        reverse ?
            std::sort(result.rbegin(), result.rend(), boost::bind(deep_less, boost::ref(trail), _1, _2)) :
            std::sort(result.begin(),  result.end(),  boost::bind(deep_less, boost::ref(trail), _1, _2));
        return result;
    }

  private:

    static boolean_type deep_less( sequence_type const& trail
                                 , value_type           a
                                 , value_type           b
                                 ) {
        return a.must_get_trail(trail) < b.must_get_trail(trail);
    }

    static sequence_type make_trail(value_type const& value) {
        string_type const source    = value.to_string();
        string_type const delimiter = text::literal(".");
        std::vector<string_type> const& names = text::split(source, delimiter);

        sequence_type trail;
        BOOST_FOREACH(string_type const& name, names) {
            trail.push_back(value_type(name));
        }
        return trail;
    }

  private:

    boolean_type          safe_;
    optional<string_type> token_;
};

}}}} // namespace ajg::synth::engines::django

#endif // AJG_SYNTH_ENGINES_DJANGO_VALUE_HPP_INCLUDED

max_stars_repo_name: legutierr/synth

max_issues_repo_name: legutierr/synth

max_forks_repo_name: legutierr/synth


text:
#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <unordered_set>
#include <vector>

#include "base/Atomic.hpp"
#include "base/TimeStamp.hpp"
#include "base/WaitGroup.hpp"
#include "db/DB.hpp"
#include "external/rapidjson/document.h"
#include "external/rapidjson/writer.h"
#include "external/rapidjson/stringbuffer.h"
#include "external/rapidjson/rapidjson.h"
#include "head/RangeHead.hpp"
#include "label/EqualMatcher.hpp"
#include "querier/BlockQuerier.hpp"
#include "querier/Querier.hpp"
#include "querier/QuerierUtils.hpp"
#include "test/TestUtils.hpp"
#include "wal/WAL.hpp"

using namespace std;
using namespace tsdb;

ulid::ULID create_block(const std::string & dir,
    const deque<tsdbutil::RefSeries> & series,
    const deque<deque<tsdbutil::RefSample>> & samples,
    const deque<tsdbutil::Stone> & stones);

deque<pair<int64_t, double>> generate_data(int64_t min, int64_t max, int64_t step);

int64_t time_delta = 30000;

void ingest_shard(db::DB * db, deque<label::Labels> * lsets, int start, int end, int64_t base_t, base::AtomicInt * total, base::WaitGroup * wg){
    vector<pair<int64_t, double>> v;
    v.reserve(end - start);
    for(int i = start; i < end; ++ i)
        v.emplace_back(-1, 123456789);
    int count = 0;
    double cd = 0;
    auto s = base::TimeStamp::now();
    for(int i = 0; i < 100; ++ i){
        auto app = db->appender();
        base_t += time_delta;
        for(int j = 0; j < v.size(); ++ j){
            v[j].second += 1000;
            if(v[j].first == -1){
                auto p = app->add((*lsets)[start + j], base_t, v[j].second);
                ASSERT_FALSE(p.second);
                v[j].first = p.first;
            }
            else{
                auto e = app->add_fast(v[j].first, base_t, v[j].second);
                ASSERT_FALSE(e);
            }
            ++ count;
        }
        auto c = base::TimeStamp::now();
        auto e = app->commit();
        cd += base::timeDifference(base::TimeStamp::now(), c);
        ASSERT_FALSE(e);
    }
    TEST_COUT << "duration=" << base::timeDifference(base::TimeStamp::now(), s) << " commit duration=" << cd << endl;
    total->add(count);
    wg->done();
}

void db_bench(){
    boost::filesystem::remove_all("db_test");

    db::DB db("db_test/bench_write");
    ASSERT_FALSE(db.error());
    deque<label::Labels> lsets;
    {
        // Benchmark write.
        {
            auto start = base::TimeStamp::now();
            ifstream file("../20kseries.json");
            string line;
            unordered_set<uint64_t> hashes;
            while(getline(file, line) && lsets.size() < 1000){
                rapidjson::Document d;
                d.Parse(line.c_str());
                label::Labels lset;
                for(auto & m : d.GetObject())
                    lset.emplace_back(m.name.GetString(), m.value.GetString());
                sort(lset.begin(), lset.end());
                bool duplicate = false;
                uint64_t hash = label::lbs_hash(lset);
                if(hashes.find(hash) == hashes.end()){
                    lsets.push_back(lset);
                    hashes.insert(hash);
                }
            }
            TEST_COUT << "> complete stage=load labels duration=" << base::timeDifference(base::TimeStamp::now(), start) << endl;
        }
        {
            base::ThreadPool pool;
            pool.start(16);
            base::WaitGroup wg;
            base::AtomicInt total;
            int scrape_count = 3000;
            auto start = base::TimeStamp::now();
            for(int i = 0; i < scrape_count; i += 100){
                int si = 0;
                while(si < lsets.size()){
                    wg.add(1);
                    pool.run(boost::bind(&ingest_shard, &db, &lsets, si, si + 1000, time_delta * i, &total, &wg));
                    // ingest_shard(&db, &lsets, si, si + 1000, time_delta * i, &total, &wg);
                    si += 1000;
                }
                wg.wait();
                // TEST_COUT << "Round " << i << " Total " << total.get() << endl;
            }
            double duration = base::timeDifference(base::TimeStamp::now(), start);
            TEST_COUT << "> complete stage=ingest duration=" << duration << endl;
            TEST_COUT << "  > total samples=" << total.get() << endl;
            TEST_COUT << "  > samples/sec=" << (double)(total.get()) / duration << endl;
        }
    }
    sleep(5);
    {
        // Benchmark query.
        auto start = base::TimeStamp::now();
        deque<pair<int64_t, int64_t>> qranges;
        for(int i = 0; i < 1000; i += 10)
            qranges.emplace_back(i * time_delta, (i + 1000) * time_delta);
        int total = 0;
        vector<pair<int64_t, double>> samples;
        samples.reserve(1000);
        deque<shared_ptr<label::MatcherInterface>> matchers;
        for(int i = 0; i < 10; ++ i)
            matchers.push_back(shared_ptr<label::MatcherInterface>(new label::EqualMatcher(lsets[i][0].label, lsets[i][0].value)));
        for(auto & r: qranges){
            auto q = db.querier(r.first, r.second);
            if(q.second){
                TEST_COUT << "Error get Querier" << endl;
                break;
            }
            for(int i = 0; i < 10; ++ i){
                samples.clear();
                auto ss = q.first->select({matchers[i]});
                if(!ss){
                    TEST_COUT << "Error select" << endl;
                    continue;
                }
                while(ss->next()){
                    auto it = ss->at()->iterator();
                    while(it->next())
                        samples.push_back(it->at());
                }
                total += samples.size();
            }
        }
        TEST_COUT << "> complete stage=query duration=" << base::timeDifference(base::TimeStamp::now(), start) << endl;
        TEST_COUT << "  > total samples=" << total << endl;
    }
}

ext: cpp

max_stars_repo_name: Jimx-/tsdb-fork

max_issues_repo_name: Jimx-/tsdb-fork

max_forks_repo_name: Jimx-/tsdb-fork


text:
#ifndef BOOST_TEST_HH
#define BOOST_TEST_HH

#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometry.hpp>

typedef boost::geometry::model::point<double, NUM_DIMS, boost::geometry::cs::cartesian> boost_point;
typedef std::pair<boost_point, size_t> boost_point_w_index;
typedef boost::geometry::model::box<boost_point> boost_bbox;
typedef std::pair<boost_bbox, size_t> boost_bbox_w_index;

inline boost_point make_boost_point(const point &pt) {
    return boost_point(pt[0], pt[1], pt[2]);
}

inline boost_bbox make_boost_bbox(const point &pt0, const point &pt1) {
    return boost_bbox(make_boost_point(pt0),make_boost_point(pt1));
}

static void print_boost_box(boost_bbox my_box) {
    std::cout << "(" << my_box.min_corner().get<0>() << " " << my_box.min_corner().get<1>() << ", " << my_box.min_corner().get<2>() << ")-";
    std::cout << "(" << my_box.max_corner().get<0>() << " " << my_box.max_corner().get<1>() << ", " << my_box.max_corner().get<2>() << ")" << std::endl;
}

static void print_boost_point(boost_point my_pt, bool suppress_newline = true) {
    std::cout << "(" << my_pt.get<0>() << " " << my_pt.get<1>() << ", " << my_pt.get<2>() << ")";
    if(! suppress_newline) {
        std::cout << std::endl;
    }
}

template <class BuildAlg>
class TestBoost : public BboxIntersectionTest {

    private:
        boost::geometry::index::rtree<boost_point_w_index, BuildAlg> *tree;
        boost::geometry::index::rtree<boost_bbox_w_index, BuildAlg> *tree_boxes;
        bool uses_boxes = false;

        struct boost_point_w_index_iterator : std::vector<boost_point_w_index>::const_iterator
        {
            using base_class = std::vector<boost_point_w_index>::const_iterator;
            using value_type = size_t;

            using base_class::base_class;

            value_type operator*() const {
                auto const& src = base_class::operator*();
                return (src.second );
            }
        };

        struct boost_bbox_w_index_iterator : std::vector<boost_bbox_w_index>::const_iterator
        {
            using base_class = std::vector<boost_bbox_w_index>::const_iterator;
            using value_type = size_t;

            using base_class::base_class;

            value_type operator*() const {
                auto const& src = base_class::operator*();
                return (src.second);
            }
        };
    public:
        bool intersections_exact() { return true; } //bounding box search

        TestBoost() {}
        ~TestBoost() {
            delete tree;
            delete tree_boxes;
        }

        void build_tree(const std::vector<point> &pts, const std::vector<size_t> &indices) {
            std::vector<boost_point_w_index> data;
            for(int i = 0; i < pts.size(); i++) {
                data.push_back(std::make_pair(make_boost_point(pts[i]),indices[i]));
            }
             tree = new boost::geometry::index::rtree<boost_point_w_index, BuildAlg>(data.begin(), data.end());
        }

        void build_tree_bbox(const std::vector<point> &pts, const std::vector<size_t> &indices) {
            if((pts.size() % 2) !=0) {
                std::cerr << "error. your point list size has to be even to insert bounding boxes" << std::endl;
                return;
            }
            uses_boxes = true;
            std::vector<boost_bbox_w_index> data;
            for(int i = 0; i < pts.size(); i += 2) {
                data.push_back(std::make_pair(make_boost_bbox(pts[i], pts[i+1]),indices[i/2]));
            }
             tree_boxes = new boost::geometry::index::rtree<boost_bbox_w_index, BuildAlg>(data.begin(), data.end());
        }

        void get_intersections(const bbox &my_bbox, std::vector<size_t> &intersections_indices) {
            if(uses_boxes) {
                std::vector<boost_bbox_w_index> results;
                tree_boxes->query(boost::geometry::index::intersects(make_boost_bbox(my_bbox.first, my_bbox.second)), std::back_inserter(results));
                intersections_indices.assign(boost_bbox_w_index_iterator(results.begin()),
                                           boost_bbox_w_index_iterator(results.end()));
            }
            else {
                std::vector<boost_point_w_index> results;
                tree->query(boost::geometry::index::intersects(make_boost_bbox(my_bbox.first, my_bbox.second)), std::back_inserter(results));
                intersections_indices.assign(boost_point_w_index_iterator(results.begin()),
                                           boost_point_w_index_iterator(results.end()));
            }
        }
};

#endif //BOOST_TEST_HH

ext: hh


text:
//
// Copyright (c) 2020 Richard Hodges (hodges.r@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/madmongo1/webclient
//
// This project was made possible with the generous support of:
// The C++ Alliance (https://cppalliance.org/)
// Jetbrains (https://www.jetbrains.com/)
//
// Talk to us on Slack (https://cppalliance.org/slack/)
//
// Many thanks to Vinnie Falco for continuous mentoring and support
//

#ifndef BOOST_WEBCLIENT_ASYNC__FUTURE__HPP
#define BOOST_WEBCLIENT_ASYNC__FUTURE__HPP

#include <boost/webclient/async/detail/future_wait_op.hpp>
#include <boost/webclient/config.hpp>
#include <boost/asio/async_result.hpp>

namespace boost { namespace webclient { namespace async {

template < class T >
struct promise;

template < class T >
struct future
{
    using impl_class = detail::future_state_impl< T >;
    using impl_type  = std::shared_ptr< impl_class >;

    using result_type = future_result_type< T >;

    template < class CompletionHandler >
    auto async_wait(CompletionHandler &&token)
        -> BOOST_ASIO_INITFN_AUTO_RESULT_TYPE(CompletionHandler, result_type);

  private:
    friend promise< T >;

    future(impl_type impl)
    : impl_(std::move(impl))
    {
    }

    impl_type impl_;
};

template < class T >
struct promise
{
    using impl_class = detail::future_state_impl< T >;
    using impl_type  = std::shared_ptr< impl_class >;

    promise();

    promise(promise &&other) noexcept;

    promise &operator=(promise &&other) noexcept;

    ~promise() noexcept;

    void set_value(T val);

    void set_error(error_code ec);

    void set_exception(std::exception_ptr ep);

    future< T > get_future();

  private:
    void destroy();

    impl_type impl_;
    impl_type future_impl_;
};

}}}   // namespace boost::webclient::async

#include "future.ipp"
#include "promise.ipp"

#endif   // BOOST_WEBCLIENT_POLYFILL_ASYNC__FUTURE__HPP