<commit_before>#include <Recognition/RecognitionData.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <opencv2/rgbd.hpp>
#include <Recognition/db_linemod.h>
#include <Recognition/linemod_icp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/point_representation.h>
//PCL ICP
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>

#include <Recognition/GLUTInit.h>

namespace Recognition{


  /***************************************************************************************************************************
   *      AS THIS WILL BE POSTED TO GITHUB SOME DAY: DEAR PERSON-WHO-WANTS-TO-HIRE-ME, READ THIS BEFORE BURNING MY CV        *
   * I'm not responsible for this static functions. It's not me who made it. Naming is horrible. Pointers do kill people.    *
   * I know this. But it's 3 days to the deadline and I don't have enough time to explain to the "developer" who made        *
   * this crap how crappy his crap is. Just don't touch it and it shall work.                                                *
   ***************************************************************************************************************************/
  bool RecognitionData::updateGiorgio(const cv::Mat& const_rgb, const cv::Mat& depth_m, const cv::Mat& filter_mask, 
      cv::Mat& pose, const std::vector<std::string>& vect_objs_to_pick) const
  {

    cv::Mat rgb=const_rgb.clone();
    CV_Assert(filter_mask.depth() == CV_8UC1);
    /** The depth_ matrix is given in m, but we want to use it in mm now */
    CV_Assert(depth_m.type() == CV_32FC1);
    cv::Mat depth_mm;
    depth_m.convertTo(depth_mm, CV_16UC1, 1000);

    std::vector<cv::Mat> sources;
    sources.push_back(rgb);
    sources.push_back(depth_mm);

    std::vector<cv::linemod::Match> nonconst_matches;
    std::vector<cv::Mat> theMasks;
    theMasks.push_back(filter_mask);
    theMasks.push_back(filter_mask);

    /** Check consistent input have been provided */
    CV_Assert(sources.size()==theMasks.size());
    for(unsigned int i=0; i<sources.size(); ++i){
      CV_Assert(sources[i].cols==theMasks[i].cols && sources[i].rows==theMasks[i].rows);
    }

    /** Create LINE-MOD detector with templates built from the object */
    cv::Ptr<cv::linemod::Detector> detector (cv::linemod::getDefaultLINEMOD());
    for(auto& object_id_ : vect_objs_to_pick){
      _objectModels.at(object_id_).addAllTemplates(*detector);
    }
    std::cout << "#Templates: " << detector->numTemplates() << "\n";

    std::cout<<"Matching..."<<"\n";
    detector->match(sources, _threshold, nonconst_matches,vect_objs_to_pick, cv::noArray(), theMasks);

    /** Just to be sure it's not changed in the Rastafari loop */
    const std::vector<cv::linemod::Match>& matches=nonconst_matches;
    std::cout<<"Done: matches.size(): "<<matches.size()<<"\n";
    if(matches.size() == 0){
      return false;
    }

    cv::Mat depth_real_ref_raw;
    cv::rgbd::depthTo3d(depth_mm, _cameraModel.getIntrinsic(), depth_real_ref_raw);
    assert(depth_real_ref_raw.type()==CV_32FC3);

    /** The buffer with detected objects and their info */
    std::vector <object_recognition_core::db::ObjData> objs_;

    /** Keep the point cloud of the best match */
    std::array< pcl::PointCloud<pcl::PointXYZRGB>::Ptr , 3 > resultPointClouds;

    for(const auto& match : matches) {

      using Eigen::Affine3f;

      // Fill the pose object
      int tId=match.template_id;
      std::cout << "Template # " << tId << " matches.\n";
      auto& obj=_objectModels.at(match.class_id);
      cv::Matx33d R_match = obj.getR(tId);
      std::cout << "Rotation matrix: \n" << R_match << "\n";
      float D_match = obj.getDist(tId);
      std::cout << "Distance:\n" << D_match << "\n";
      const auto& K_match = obj.getK(tId);

      //get the point cloud of the rendered object model
      cv::Mat mask;
      cv::Rect rect;
      /*
      auto it_r = _objectModels.at(match.class_id).getRenderer();
      cv::Mat depth_ref_;
      it_r->renderDepthOnly(depth_ref_, mask, rect, -T_match, up);

      cv::Mat_<cv::Vec3f> depth_real_model_raw;
      cv::rgbd::depthTo3d(depth_ref_, K_match, depth_real_model_raw);

      //prepare the bounding box for the model and reference point clouds
      cv::Rect_<int> rect_model(0, 0, depth_real_model_raw.cols, depth_real_model_raw.rows);
      //prepare the bounding box for the reference point cloud: add the offset
      cv::Rect_<int> rect_ref(rect_model);
      rect_ref.x += match.x;
      rect_ref.y += match.y;

      rect_ref = rect_ref & cv::Rect(0, 0, depth_real_ref_raw.cols, depth_real_ref_raw.rows);
      if ((rect_ref.width < 5) || (rect_ref.height < 5))
        continue;
      //adjust both rectangles to be equal to the smallest among them
      if (rect_ref.width > rect_model.width)
        rect_ref.width = rect_model.width;
      if (rect_ref.height > rect_model.height)
        rect_ref.height = rect_model.height;
      if (rect_model.width > rect_ref.width)
        rect_model.width = rect_ref.width;
      if (rect_model.height > rect_ref.height)
        rect_model.height = rect_ref.height;

      //prepare the reference data: from the sensor : crop images
      cv::Mat_<cv::Vec3f> depth_real_ref = depth_real_ref_raw(rect_ref);

      //plot Red Rect around the match in the full image
      cv::rectangle(rgb,rect_ref,cv::Scalar(0,0,255),2);

      cv::imshow("rgb",rgb);

      cv::waitKey(1);

      //prepare the model data: from the match
      cv::Mat_<cv::Vec3f> depth_real_model = depth_real_model_raw(rect_model);

      //initialize the translation based on reference data
      cv::Vec3f T_crop = depth_real_ref(depth_real_ref.rows / 2.0f, depth_real_ref.cols / 2.0f);
      //add the object's depth
      T_crop(2) += D_match;

      if (!cv::checkRange(T_crop))
        continue;
      cv::Vec3f T_real_icp(T_crop);

      //initialize the rotation based on model data
      if (!cv::checkRange(R_match))
        continue;
      cv::Matx33f R_real_icp(R_match);

      //get the point clouds (for both reference and model)
      std::vector<cv::Vec3f> pts_real_model_temp;
      std::vector<cv::Vec3f> pts_real_ref_temp;
      double px_ratio_missing = matToVec(depth_real_model, depth_real_ref, pts_real_model_temp, pts_real_ref_temp);
      std::cout<<"px_ratio_missing > px_match_min_ ?: "<<px_ratio_missing<<" > " <<  px_match_min_ <<"\n";
      if (px_ratio_missing > px_match_min_)
        continue;

      Eigen::Matrix4f finalTransformationMatrix;
      //TODO if(!pclICP(pts_real_model_temp, pts_real_ref_temp, finalTransformationMatrix, resultPointClouds)){
      //TODO   continue;
      //TODO }

      */
      /** Take the best match and return it as a position */

      /** Fill the transformation matrix */
      Eigen::Matrix3d eRot;

      cv2eigen(R_match, eRot);

      Eigen::Affine3d matchTrans=Eigen::Affine3d::Identity();

      matchTrans.translation() << 0,0,D_match;
      matchTrans.linear()=eRot;
      const auto& Cam_match = obj.getCam(tId);
      Eigen::Affine3d cameraToWorld(Cam_match.getExtrinsic().inverse());

      /** Move to global positions */
      std::cout << "Matrix0: \n" <<
        matchTrans.matrix() << "\n";
      Eigen::Affine3d mattiaEUnTrans(matchTrans);
      matchTrans = cameraToWorld * matchTrans;

      std::cout << "Matrix: \n" <<
        matchTrans.matrix() << "\n";
      cv::Mat sticazzi, stimazzi, stimaski;
      cv::Mat newFrame=const_rgb.clone();
      cv::Rect stiretti;
      obj.render(mattiaEUnTrans, stimazzi, sticazzi, stimaski, stiretti);
      stiretti.x=match.x;
      stiretti.y=match.y;
      stimazzi.copyTo(newFrame.rowRange(stiretti.y, stiretti.y+stiretti.height).colRange(stiretti.x,stiretti.x+stiretti.width));

      eigen2cv(matchTrans.matrix(), pose);
      imshow("Sbarubba", newFrame);
      while((cv::waitKey() & 0xFF)!='q');<commit_msg>Add false positive test for linemod based on (downsampled) hue value<commit_after>#include <cmath>
#include <Recognition/RecognitionData.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <opencv2/rgbd.hpp>
#include <Recognition/db_linemod.h>
#include <Recognition/linemod_icp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/point_representation.h>
//PCL ICP
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>

#include <Recognition/GLUTInit.h>

namespace Recognition{

  static void turnBlackWhiteToBlueYellow(const cv::Mat& hsv_in, cv::Mat& hsv_out, double ts, double tv){
    using cv::Mat;
    Mat whiteMask, blackMask;
    Mat allY(hsv_in.size(), CV_8UC3);
    allY.setTo(cv::Scalar{30,255,255});
    Mat allB(hsv_in.size(), CV_8UC3);
    allB.setTo(cv::Scalar{120,255,255});

    /** Turn black points to blue points and white points to yellow points*/
    cv::inRange(hsv_in, cv::Scalar{0,0,0}, cv::Scalar{255, 255, tv}, blackMask);
    cv::inRange(hsv_in, cv::Scalar{0,0,tv}, cv::Scalar{255, ts, 255}, whiteMask);

    hsv_out=hsv_in.clone();
    allY.copyTo(hsv_out, whiteMask);
    allB.copyTo(hsv_out, blackMask);

  }

  static double matchingHuePercentage(const cv::Mat& matchingTemplate, const cv::Mat& possibleMatch, const cv::Mat& mask, const cv::Size& matchSize, double cutPercentage, double acceptThreshold, double scaleFactor){
    using cv::Mat;
    using cv::Rect;
    using cv::Size;

    /** Blank the background */
    Mat blankedTemplate(matchSize, CV_8UC3), blankedMatch(matchSize, CV_8UC3);
    blankedTemplate.setTo(cv::Scalar{0,0,0});
    blankedMatch.setTo(cv::Scalar{0,0,0});

    matchingTemplate.copyTo(blankedTemplate, mask);
    possibleMatch.copyTo(blankedMatch, mask);
    /** Remove a small border from the match, in order to compensate wrong-by-little positioning */
    cv::Rect section;
    section.width=0.8*matchSize.width;
    section.height=0.8*matchSize.height;
    section.y=0.1*matchSize.height;
    section.x=0.1*matchSize.width;
    Mat templatePart=blankedTemplate(section);
    Mat matchPart=blankedMatch(section);
    Mat maskPart=mask(section);
    Mat hsvTemplate, hsvMatch, filteredTemplate, filteredMatch;
    cv::cvtColor(templatePart, hsvTemplate, CV_BGR2HSV);
    cv::cvtColor(matchPart, hsvMatch, CV_BGR2HSV);
    turnBlackWhiteToBlueYellow(hsvTemplate, filteredTemplate, 30, 30);
    turnBlackWhiteToBlueYellow(hsvMatch, filteredMatch, 30, 30);
    Mat displayT, displayM;
    cv::cvtColor(hsvTemplate, displayT, CV_HSV2BGR);
    cv::cvtColor(hsvMatch, displayM, CV_HSV2BGR);

    int totalPoints, matchingPoints;
    Mat scaledTemplate, scaledMatch;
    cv::resize(filteredTemplate, scaledTemplate, cv::Size(0,0), 1.0/scaleFactor, 1.0/scaleFactor);
    cv::resize(filteredMatch, scaledMatch, cv::Size(0,0), 1.0/scaleFactor, 1.0/scaleFactor);
    Mat matchingDraw(scaledTemplate.size(), CV_8UC1);
    matchingDraw.setTo(0);
    for(int i=0; i<filteredTemplate.cols; ++i){
      for(int j=0; j<filteredTemplate.rows; ++j){
        if(maskPart.at<unsigned char>(i*scaleFactor, j*scaleFactor)){
          totalPoints++;
          if(fabs(scaledTemplate.at<cv::Vec3b>(i,j)[0]-scaledMatch.at<cv::Vec3b>(i,j)[0])<acceptThreshold){
            matchingPoints++;
            matchingDraw.at<unsigned char>(i,j)=255;
          }
        }
      }
    }

    cv::imshow("T", displayT);
    cv::imshow("M", displayM);
    cv::imshow("D", matchingDraw);
    while((cv::waitKey() & 0xFF)!='q');
    cv::destroyWindow("T");
    cv::destroyWindow("M");
    cv::destroyWindow("D");
    return 0;
  }

  /***************************************************************************************************************************
   *      AS THIS WILL BE POSTED TO GITHUB SOME DAY: DEAR PERSON-WHO-WANTS-TO-HIRE-ME, READ THIS BEFORE BURNING MY CV        *
   * I'm not responsible for this static functions. It's not me who made it. Naming is horrible. Pointers do kill people.    *
   * I know this. But it's 3 days to the deadline and I don't have enough time to explain to the "developer" who made        *
   * this crap how crappy his crap is. Just don't touch it and it shall work.                                                *
   ***************************************************************************************************************************/
  bool RecognitionData::updateGiorgio(const cv::Mat& const_rgb, const cv::Mat& depth_m, const cv::Mat& filter_mask, 
      cv::Mat& pose, const std::vector<std::string>& vect_objs_to_pick) const
  {

    using cv::Rect;
    using cv::Mat;
    Mat rgb=const_rgb.clone();
    CV_Assert(filter_mask.depth() == CV_8UC1);
    /** The depth_ matrix is given in m, but we want to use it in mm now */
    CV_Assert(depth_m.type() == CV_32FC1);
    Mat depth_mm;
    depth_m.convertTo(depth_mm, CV_16UC1, 1000);

    std::vector<Mat> sources;
    sources.push_back(rgb);
    sources.push_back(depth_mm);

    std::vector<cv::linemod::Match> nonconst_matches;
    std::vector<Mat> theMasks;
    theMasks.push_back(filter_mask);
    theMasks.push_back(filter_mask);

    /** Check consistent input have been provided */
    CV_Assert(sources.size()==theMasks.size());
    for(unsigned int i=0; i<sources.size(); ++i){
      CV_Assert(sources[i].cols==theMasks[i].cols && sources[i].rows==theMasks[i].rows);
    }

    /** Create LINE-MOD detector with templates built from the object */
    cv::Ptr<cv::linemod::Detector> detector (cv::linemod::getDefaultLINEMOD());
    for(auto& object_id_ : vect_objs_to_pick){
      _objectModels.at(object_id_).addAllTemplates(*detector);
    }
    std::cout << "#Templates: " << detector->numTemplates() << "\n";

    std::cout<<"Matching..."<<"\n";
    detector->match(sources, _threshold, nonconst_matches,vect_objs_to_pick, cv::noArray(), theMasks);

    /** Just to be sure it's not changed in the Rastafari loop */
    const std::vector<cv::linemod::Match>& matches=nonconst_matches;
    std::cout<<"Done: matches.size(): "<<matches.size()<<"\n";
    if(matches.size() == 0){
      return false;
    }

    Mat depth_real_ref_raw;
    cv::rgbd::depthTo3d(depth_mm, _cameraModel.getIntrinsic(), depth_real_ref_raw);
    assert(depth_real_ref_raw.type()==CV_32FC3);

    /** The buffer with detected objects and their info */
    std::vector <object_recognition_core::db::ObjData> objs_;

    /** Keep the point cloud of the best match */
    std::array< pcl::PointCloud<pcl::PointXYZRGB>::Ptr , 3 > resultPointClouds;

    for(const auto& match : matches) {

      using Eigen::Affine3f;

      // Fill the pose object
      int tId=match.template_id;
      std::cout << "Template # " << tId << " matches.\n";
      auto& obj=_objectModels.at(match.class_id);
      cv::Matx33d R_match = obj.getR(tId);
      std::cout << "Rotation matrix: \n" << R_match << "\n";
      float D_match = obj.getDist(tId);
      std::cout << "Distance:\n" << D_match << "\n";
      const auto& K_match = obj.getK(tId);

      //get the point cloud of the rendered object model
      Mat mask;
      Rect rect;
      /*
      auto it_r = _objectModels.at(match.class_id).getRenderer();
      Mat depth_ref_;
      it_r->renderDepthOnly(depth_ref_, mask, rect, -T_match, up);

      cv::Mat_<cv::Vec3f> depth_real_model_raw;
      cv::rgbd::depthTo3d(depth_ref_, K_match, depth_real_model_raw);

      //prepare the bounding box for the model and reference point clouds
      cv::Rect_<int> rect_model(0, 0, depth_real_model_raw.cols, depth_real_model_raw.rows);
      //prepare the bounding box for the reference point cloud: add the offset
      cv::Rect_<int> rect_ref(rect_model);
      rect_ref.x += match.x;
      rect_ref.y += match.y;

      rect_ref = rect_ref & Rect(0, 0, depth_real_ref_raw.cols, depth_real_ref_raw.rows);
      if ((rect_ref.width < 5) || (rect_ref.height < 5))
        continue;
      //adjust both rectangles to be equal to the smallest among them
      if (rect_ref.width > rect_model.width)
        rect_ref.width = rect_model.width;
      if (rect_ref.height > rect_model.height)
        rect_ref.height = rect_model.height;
      if (rect_model.width > rect_ref.width)
        rect_model.width = rect_ref.width;
      if (rect_model.height > rect_ref.height)
        rect_model.height = rect_ref.height;

      //prepare the reference data: from the sensor : crop images
      cv::Mat_<cv::Vec3f> depth_real_ref = depth_real_ref_raw(rect_ref);

      //plot Red Rect around the match in the full image
      cv::rectangle(rgb,rect_ref,cv::Scalar(0,0,255),2);

      cv::imshow("rgb",rgb);

      cv::waitKey(1);

      //prepare the model data: from the match
      cv::Mat_<cv::Vec3f> depth_real_model = depth_real_model_raw(rect_model);

      //initialize the translation based on reference data
      cv::Vec3f T_crop = depth_real_ref(depth_real_ref.rows / 2.0f, depth_real_ref.cols / 2.0f);
      //add the object's depth
      T_crop(2) += D_match;

      if (!cv::checkRange(T_crop))
        continue;
      cv::Vec3f T_real_icp(T_crop);

      //initialize the rotation based on model data
      if (!cv::checkRange(R_match))
        continue;
      cv::Matx33f R_real_icp(R_match);

      //get the point clouds (for both reference and model)
      std::vector<cv::Vec3f> pts_real_model_temp;
      std::vector<cv::Vec3f> pts_real_ref_temp;
      double px_ratio_missing = matToVec(depth_real_model, depth_real_ref, pts_real_model_temp, pts_real_ref_temp);
      std::cout<<"px_ratio_missing > px_match_min_ ?: "<<px_ratio_missing<<" > " <<  px_match_min_ <<"\n";
      if (px_ratio_missing > px_match_min_)
        continue;

      Eigen::Matrix4f finalTransformationMatrix;
      //TODO if(!pclICP(pts_real_model_temp, pts_real_ref_temp, finalTransformationMatrix, resultPointClouds)){
      //TODO   continue;
      //TODO }

      */
      /** Take the best match and return it as a position */

      /** Fill the transformation matrix */
      Eigen::Matrix3d eRot;

      cv2eigen(R_match, eRot);

      Eigen::Affine3d matchTrans=Eigen::Affine3d::Identity();

      matchTrans.translation() << 0,0,D_match;
      matchTrans.linear()=eRot;
      const auto& Cam_match = obj.getCam(tId);
      Eigen::Affine3d cameraToWorld(Cam_match.getExtrinsic().inverse());

      /** Move to global positions */
      std::cout << "Matrix0: \n" <<
        matchTrans.matrix() << "\n";
      Eigen::Affine3d mattiaEUnTrans(matchTrans);
      matchTrans = cameraToWorld * matchTrans;

      std::cout << "Matrix: \n" <<
        matchTrans.matrix() << "\n";
      Mat sticazzi, stimazzi, stimaski;
      Mat newFrame=const_rgb.clone();
      Rect stiretti;
      obj.render(mattiaEUnTrans, stimazzi, sticazzi, stimaski, stiretti);
      stiretti.x=match.x;
      stiretti.y=match.y;
      Mat matchingPart=newFrame(stiretti);
      if(matchingHuePercentage(stimazzi, matchingPart, stimaski, stiretti.size(), 0.1,10,2) < 0.7) {
        std::cout << "Object rejected. Trying another template...\n";
        continue;
      }

      /** Check for false positives by valuating hues values on the downsampled image */
      eigen2cv(matchTrans.matrix(), pose);
      imshow("Sbarubba", newFrame);
      while((cv::waitKey() & 0xFF)!='q');
{'fraction_non_alphanumeric': 0.15915024315331455, 'fraction_numerical': 0.012029690299462503, 'mean_word_length': 3.620624408703879, 'pattern_counts': {'":': 0, '<': 204, '<?xml version=': 0, '>': 96, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 4, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1669565', 'n_tokens_mistral': 6792, 'n_tokens_neox': 6532, 'n_words': 1397}