filter_jump_edge_alg.cpp

Go to the documentation of this file.

#include "filter_jump_edge_alg.h"

FilterJumpEdgeAlgorithm::FilterJumpEdgeAlgorithm(void)
{
}

FilterJumpEdgeAlgorithm::~FilterJumpEdgeAlgorithm(void)
{
}

void FilterJumpEdgeAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// FilterJumpEdgeAlgorithm Public API

bool FilterJumpEdgeAlgorithm::apply_jump_edge_filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  // Convert from msg to algorithm based inputs                                  
  //pcl::fromROSMsg(*msg, filtered_pc_);                                                 
  //pcl::fromROSMsg(*msg, residual_pc_);                                                 
  filtered_pc_ = *msg;
  residual_pc_ = *msg;

  // Jump Edge depth image
  residual_img_.header          = msg->header;
  residual_img_.height          = msg->height;
  residual_img_.width           = msg->width;
  residual_img_.encoding        = sensor_msgs::image_encodings::MONO8;
  residual_img_.step            = msg->width;
  residual_img_.data.resize(msg->width * msg->height);

  // [JUMP EDGE FILTER ALGORITHM]

  // focal distance
  float fd = sx_/(0.5*msg->width/u0_);
  // X and Y pixel scale
  float mx = (0.5*msg->width/u0_);
  float my = (0.5*msg->height/u0_);
  // Angle of incidence in X coords
  float apex_X = atan(mx/fd);
  // Angle of incidence in Y coords
  float apex_Y = atan(my/fd);
  // Angle of incidence in the diagonal XY.
  // apex = 2.0*atan(0.5*n_cc/sx_)/n_cc;
  float apex_XY = atan((1/sqrt(pow(mx,2)+pow(my,2)))/fd);

  //pcl::PointCloud<pcl::PointXYZRGB>::iterator fil_it = filtered_pc_.begin();
  //pcl::PointCloud<pcl::PointXYZRGB>::iterator res_it = residual_pc_.begin();
  
  for (unsigned int rr=0; rr<msg->height; ++rr){
    //for (unsigned int cc=0; cc<msg->width; ++cc, ++fil_it, ++res_it){
    for (unsigned int cc=0; cc<msg->width; ++cc){
      int idx0 = rr*msg->width + cc;
      //residual_img_.data[idx0] = 0.0; // Initialize image to black 
      //residual_img_.data[idx0] = fil_it->r; // Initialize image to previous input pcl image 
      float *fil_step = (float*)&filtered_pc_.data[idx0 * filtered_pc_.point_step];
      float *res_step = (float*)&residual_pc_.data[idx0 * residual_pc_.point_step];
      residual_img_.data[idx0] = 255*fil_step[3]/50000; // Initialize image to previous input pcl image 

      // Bounding Box Threshold
      // std::cout << std::endl << " z_e: " << this->z_e;
      // if ( x_ < this->x_s || y_ < this->y_s || z_ < this->z_s || x_ > this->x_e || y_ > this->y_e || z_ > this->z_e) 
      //   nstep[0] = nstep[1] = nstep[2] = nstep[3] = std::numeric_limits<float>::quiet_NaN ();
      
      //if (rr==0 || rr == msg->height-1 || cc == 0 || cc == msg->width-1 || fil_it->z == 0.0){
      if (rr==0 || rr == msg->height-1 || cc == 0 || cc == msg->width-1 || fil_step[2] == 0.0){
        //fil_it->z = std::numeric_limits<float>::quiet_NaN ();
        fil_step[2] = std::numeric_limits<float>::quiet_NaN ();
        residual_img_.data[idx0] = 0.0; // Initialize image to black 
        filtered_pc_.is_dense = false;
        continue;
      }
      
      Eigen::Vector3f Va;
      //Va(0) = fil_it->z*(cc - u0_)/sx_;
      //Va(1) = fil_it->z*(rr - v0_)/sy_;
      //Va(2) = fil_it->z;
      Va(0) = fil_step[2]*(cc - u0_)/sx_;
      Va(1) = fil_step[2]*(rr - v0_)/sy_;
      Va(2) = fil_step[2];
      int kk=0;
      Eigen::ArrayXf neigh_ang(8);
      
      for (int ii=-1; ii < 2; ii++){
        for (int jj=-1; jj < 2; jj++){
          if (ii==0 && jj==0)
            continue;
          int tr = rr + ii;
          int tc = cc + jj;
          int idx1 = tr*msg->width + tc;
          float *pstep1 = (float*)&msg->data[idx1 * msg->point_step];
          float tmp_value1 = pstep1[2];
          Eigen::Vector3f Vb;
          Vb(0) = tmp_value1*(tc - u0_)/sx_;
          Vb(1) = tmp_value1*(tr - v0_)/sy_;
          Vb(2) = tmp_value1;
          float modVectNeigh = Vb.norm();
          float modVectDif = (Va-Vb).norm();
          if (ii==jj) {
            neigh_ang(kk) = 180.0/M_PI * asin(sin(apex_XY)*(modVectNeigh)/modVectDif);
          } else if (ii==0) {
            neigh_ang(kk) = 180.0/M_PI * asin(sin(apex_Y)*(modVectNeigh)/modVectDif);
          } else if (jj==0) {
            neigh_ang(kk) = 180.0/M_PI * asin(sin(apex_X)*(modVectNeigh)/modVectDif);
          } else {
            neigh_ang(kk) = 180.0/M_PI * asin(sin(apex_XY)*(modVectNeigh)/modVectDif);
          }
          kk++;
        }
      }
      // IF it is a flying point
      if (neigh_ang.minCoeff() < ang_thr_){
        residual_img_.data[idx0] = 0.0;
        //fil_it->z = std::numeric_limits<float>::quiet_NaN ();
        //fil_it->rgb = 0.0;
        fil_step[2] = std::numeric_limits<float>::quiet_NaN ();
        fil_step[3] = 0.0;
        filtered_pc_.is_dense = false;
      // ELSE
      } else {
//        residual_img_.data[idx0] = 255.0;
        //res_it->z = std::numeric_limits<float>::quiet_NaN ();
        res_step[2] = std::numeric_limits<float>::quiet_NaN ();
        residual_pc_.is_dense = false;
      }
    }
  }

  return true;
}

sensor_msgs::PointCloud2* FilterJumpEdgeAlgorithm::get_filtered_pc(void)
{
  return &filtered_pc_;
}

sensor_msgs::PointCloud2* FilterJumpEdgeAlgorithm::get_residual_pc(void)
{
  return &residual_pc_;
}

sensor_msgs::Image* FilterJumpEdgeAlgorithm::get_residual_img(void)
{
  return &residual_img_;
}

void FilterJumpEdgeAlgorithm::set_sx(double sx)
{
  sx_ = sx;
}

void FilterJumpEdgeAlgorithm::set_sy(double sy)
{
  sy_ = sy;
}

void FilterJumpEdgeAlgorithm::set_u0(double u0)
{
  u0_ = u0;
}

void FilterJumpEdgeAlgorithm::set_v0(double v0)
{
  v0_ = v0;
}

void FilterJumpEdgeAlgorithm::set_skw(double skw)
{
  skw_ = skw;
}

void FilterJumpEdgeAlgorithm::set_ang_thr(double ang_thr)
{
  ang_thr_ = ang_thr;
}