missionManagerVision.cpp

#include "header/missionManagerVision.h"


int main(int argc, char** argv){

  if (argc < 2){
    std::cout << "[MISSION_MANAGER] Please insert robot name for Vision"<< std::endl;
    return -1;
  }

  boost::filesystem::path path(__FILE__);
  path.remove_filename();
  std::string sourcePath = path.string();

  std::string robotName = argv[1];
  std::string pathLog;
  if (LOG && (argc > 2)){   //if flag log setted to 1 and path log is given
    pathLog = argv[2];
  }

  ros::init(argc, argv, robotName + "_MissionManagerVision");
  std::cout << "[" << robotName << "][MISSION_MANAGER] Start" << std::endl;
  ros::NodeHandle nh;

  InfosVision robVisInfo;

  RobotVisionInterface robotVisInterface(nh, robotName);
  robotVisInterface.init();

  std::string cameraLName = "bowtechL_C";
  std::string cameraRName = "bowtechR_C";
  std::string cameraRangeRName = "bowtechRangeR_C";
  std::string holeName = "hole";
  WorldInterface worldInterface(robotName);
  worldInterface.waitReady(holeName);

  VisionInterfaceVision visionInterface(nh, robotName);


  robotVisInterface.getwTv(&(robVisInfo.robotState.wTv_eigen));

  worldInterface.getT(&robVisInfo.robotStruct.vTcameraL, robotName, cameraLName);
  worldInterface.getT(&robVisInfo.robotStruct.vTcameraR, robotName, cameraRName);
  worldInterface.getT(&robVisInfo.robotStruct.cLTcR, cameraLName, cameraRName);
  worldInterface.getT(&robVisInfo.robotStruct.cRTcL, cameraRName, cameraLName);
  worldInterface.getT(&robVisInfo.robotStruct.vTcameraRangeR, robotName, cameraRangeRName);
  worldInterface.getT(&robVisInfo.robotStruct.cRangeRTcL, cameraRangeRName, cameraLName);

  // real hole pose to log errors of pose estimation
  worldInterface.getwT(&(robVisInfo.transforms.wTh_eigen), holeName);




  Logger logger;
  if (pathLog.size() > 0){
    logger = Logger(robotName, pathLog);
    logger.createDirectoryForNode();
  }



  vpImage<unsigned char> imageL_vp, imageR_vp, imageRangeR_vp;
  cv::Mat imageL_cv, imageR_cv, imageRangeR_cv;

  robotVisInterface.getLeftImage(&imageL_cv);
  robotVisInterface.getRightImage(&imageR_cv);
  imageL_cv.convertTo(imageL_cv, CV_8U); //TODO check if necessary convert in 8U
  imageR_cv.convertTo(imageR_cv, CV_8U); //TODO check if necessary convert in 8U

  vpImageConvert::convert(imageL_cv, imageL_vp);
  vpImageConvert::convert(imageR_cv, imageR_vp);

  //DEPTH camera
  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
  if (USE_DEPTH){

    robotVisInterface.getRangeRightImage(&imageRangeR_cv);
    imageRangeR_cv.convertTo(imageRangeR_cv, CV_16UC1);
    // convert function of visp dont convert from cv to uint16_t visp matrix)
    vpImage<uint16_t> imageRangeR_vp_raw;
    imageRangeR_vp_raw.resize(imageRangeR_cv.rows, imageRangeR_cv.cols);
    for (int i=0; i<imageRangeR_cv.rows; i++){
      for (int j=0; j<imageRangeR_cv.cols; j++){
        imageRangeR_vp_raw[i][j] = imageRangeR_cv.at<unsigned short>(i,j);
      }
    }
    //DCAM::makePointCloud(imageRangeR_vp_raw, pointcloud);
    vpImageConvert::createDepthHistogram(imageRangeR_vp_raw, imageRangeR_vp);

    DCAM::makePointCloud(imageRangeR_cv, pointcloud);
  }


  vpDisplayOpenCV display_left;
  vpDisplayOpenCV display_right;
  vpDisplayOpenCV display_rangeRight;
  display_left.setDownScalingFactor(vpDisplay::SCALE_AUTO);
  display_right.setDownScalingFactor(vpDisplay::SCALE_AUTO);
  display_rangeRight.setDownScalingFactor(vpDisplay::SCALE_AUTO);
  display_left.init(imageL_vp, 70, 750, "Model-based tracker (Left)");
  display_right.init(imageR_vp, 440, 750, "Model-based tracker (Right)");
  if (USE_DEPTH){
    display_rangeRight.init(imageRangeR_vp, 120+(int)imageL_vp.getWidth()+(int)imageR_vp.getWidth(),
                            100, "Model-based tracker (Range Right)");
    vpDisplay::display(imageRangeR_vp);
    vpDisplay::flush(imageRangeR_vp);
  }

  std::vector<std::string> cameraNames(2); //for config files
  cameraNames.at(0) = "left";
  if (USE_DEPTH){
    cameraNames.at(1) = "rangeRight";
  } else {
    cameraNames.at(1) = "right";
  }

  MonoTracker* monoTrackerL;
  MonoTracker* monoTrackerR;
  StereoTracker* stereoTracker;
  std::map<std::string, vpHomogeneousMatrix> mapCameraTransf;
  mapCameraTransf[cameraNames.at(0)] = vpHomogeneousMatrix();  //identity

  // note: here it must be putted the transf from RIGTH to LEFT and not viceversa
  if (!USE_DEPTH){
    mapCameraTransf[cameraNames.at(1)] =
      CONV::transfMatrix_eigen2visp(robVisInfo.robotStruct.cRTcL);
  } else {
    mapCameraTransf[cameraNames.at(1)] =
        CONV::transfMatrix_eigen2visp(robVisInfo.robotStruct.cRangeRTcL);
  }

  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
  mapOfImages[cameraNames.at(0)] = &imageL_vp;
  if (!USE_DEPTH){
    mapOfImages[cameraNames.at(1)] = &imageR_vp;
  } else {
    mapOfImages[cameraNames.at(1)] = &imageRangeR_vp;
  }

  // initialize trasformation matrix from camera to object
  std::map<std::string, vpHomogeneousMatrix> mapOfcameraToObj;
  mapOfcameraToObj[cameraNames.at(0)] = vpHomogeneousMatrix();
  mapOfcameraToObj[cameraNames.at(1)] = vpHomogeneousMatrix();

  if (TRACK_METHOD == 0){  
    monoTrackerL = new MonoTracker(robotName, cameraNames.at(0),
                     vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);
    monoTrackerR = new MonoTracker(robotName, cameraNames.at(1),
                     vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);

  } else if (TRACK_METHOD == 1) {
    std::vector<int> trackerTypes;
    trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);
    if (!USE_DEPTH){
      trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);
    } else {
      trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
    }

    stereoTracker = new StereoTracker(robotName, cameraNames, mapCameraTransf, trackerTypes);
  } else {
    std::cerr << "[" << robotName << "][MISSION_MANAGER] NOT RECOGNIZED METHOD " << TRACK_METHOD <<
              " for tracking Phase"<< std::endl;
    return -1;
  }


  std::vector<std::vector<cv::Point>> found4CornersVectorL, found4CornersVectorR; //used if initclick false
  if (DETECT_METHOD == 0){ //init by click method
    if (TRACK_METHOD == 0){ //mono
      monoTrackerL->initTrackingByClick(&imageL_vp);
      monoTrackerR->initTrackingByClick(&imageR_vp);

    } else { //stereo (depth or not depth here is the same
      stereoTracker->initTrackingByClick(mapOfImages);
    }


  } else { //find point without click //TODO, parla anche di altri metodi provati

    if (USE_DEPTH){
      if (DETECT_METHOD == 1){       
        Detector::findSquare(imageL_cv, &found4CornersVectorL);
        Detector::drawSquares(imageL_cv, found4CornersVectorL, "left");

      } else if (DETECT_METHOD == 2){       
          cv::Mat templL1 = cv::imread(sourcePath + templName);
          found4CornersVectorL.resize(1);
          Detector::templateMatching(imageL_cv, templL1, &(found4CornersVectorL.at(0)));

      } else {
        std::cerr << "[" << robotName << "][MISSION_MANAGER] NOT RECOGNIZED METHOD " << DETECT_METHOD <<
                  " for detection Phase"<< std::endl;
        return -1;

      }

      append2Dto3Dfile(found4CornersVectorL.at(0), sourcePath);

    } else {  // no depth use

      if (DETECT_METHOD == 1){       
        //https://docs.opencv.org/3.4/db/d00/samples_2cpp_2squares_8cpp-example.html#a20
        Detector::findSquare(imageL_cv, &found4CornersVectorL);
        Detector::findSquare(imageR_cv, &found4CornersVectorR);

        cv::imshow("Left Detection", imageL_cv);
        cv::moveWindow("Left Detection", 60,80);
        cv::imshow("Right Detection", imageR_cv);
        cv::moveWindow("Right Detection", 430,80);
        cv::waitKey(0);

        Detector::drawSquares(imageL_cv, found4CornersVectorL, "Left Detection");
        Detector::drawSquares(imageR_cv, found4CornersVectorR, "Right Detection");

      } else if (DETECT_METHOD == 2){       

        // https://docs.opencv.org/3.4.6/de/da9/tutorial_template_matching.html
        //TODO usare diversi template da diverse angolazioni...
        // scrivi che template va anche bene però se simoa storti rispetto
        // al hole non riusciamo a fare un buon contorno perchè la regione visibile
        // non è un quadrato giusto e non possiamo sapere le dimensioni dei lati
        // in pixel nella immagine che si vede dalle camere... ci vorrebbe homography
        // per rectify immagini...
        //TODO rectify?
        cv::Mat templL1 = cv::imread(sourcePath + templName);
        cv::Mat templR1 = cv::imread(sourcePath + templName);
        found4CornersVectorL.resize(1);
        found4CornersVectorR.resize(1);
        Detector::templateMatching(imageL_cv, templL1, &(found4CornersVectorL.at(0)));
        Detector::templateMatching(imageR_cv, templR1, &(found4CornersVectorR.at(0)));

      } else {
        std::cerr << "[" << robotName << "][MISSION_MANAGER] NOT RECOGNIZED METHOD " << DETECT_METHOD <<
                  " for detection Phase"<< std::endl;
        return -1;
      }


      //TODO  if found4CornersVector contain more than one element, pick the best...
      if (found4CornersVectorR.size() < 1){
        std::cout << "[" << robotName << "][MISSION_MANAGER] ERROR: NOT FOUND ANY SQUARE IN RIGHT IMAGE" << std::endl;
        return -1;
      }
      if (found4CornersVectorL.size() < 1){
        std::cout << "[" << robotName << "][MISSION_MANAGER] ERROR: NOT FOUND ANY SQUARE IN LEFT IMAGE" << std::endl;
        return -1;
      }

      append2Dto3Dfile(found4CornersVectorL.at(0), found4CornersVectorR.at(0), sourcePath);
    }



    if (TRACK_METHOD == 0){
      monoTrackerL->initTrackingByPoint(&imageL_vp);
      monoTrackerR->initTrackingByPoint(&imageR_vp);

    } else { //stereo, depth or not does not matter for this initiTracking
      stereoTracker->initTrackingByPoint(mapOfImages);

    }
  }


  //get camera param for display purpose
  vpCameraParameters cam_left, cam_right;
  if (TRACK_METHOD == 0){
    monoTrackerL->getCameraParams(&cam_left);
    monoTrackerR->getCameraParams(&cam_right);

  } else { //stereo
    std::map<std::string, vpCameraParameters> mapOfCamParams;
    stereoTracker->getCamerasParams(&mapOfCamParams);
    cam_left = mapOfCamParams.at(cameraNames.at(0));
    cam_right = mapOfCamParams.at(cameraNames.at(1));
  }

  std::cout << "[" << robotName << "][MISSION_MANAGER] Starting loop" << std::endl;
  ros::Rate loop_rate(10);
  while (ros::ok()) {
    robotVisInterface.getwTv(&(robVisInfo.robotState.wTv_eigen));

    robotVisInterface.getLeftImage(&imageL_cv);
    robotVisInterface.getRightImage(&imageR_cv);
    imageL_cv.convertTo(imageL_cv, CV_8U); //TODO check if necessary convert in 8U
    imageR_cv.convertTo(imageR_cv, CV_8U); //TODO check if necessary convert in 8U
    vpImageConvert::convert(imageL_cv, imageL_vp);
    vpImageConvert::convert(imageR_cv, imageR_vp);

    vpDisplay::display(imageL_vp);
    vpDisplay::display(imageR_vp);

    if (USE_DEPTH){
      robotVisInterface.getRangeRightImage(&imageRangeR_cv);
      imageRangeR_cv.convertTo(imageRangeR_cv, CV_16UC1);

      // convert function of visp dont convert from cv to uint16_t visp matrix)
      vpImage<uint16_t> imageRangeR_vp_raw;
      imageRangeR_vp_raw.resize(imageRangeR_cv.rows, imageRangeR_cv.cols);
      for (int i=0; i<imageRangeR_cv.rows; i++){
        for (int j=0; j<imageRangeR_cv.cols; j++){
          imageRangeR_vp_raw[i][j] = imageRangeR_cv.at<unsigned short>(i,j);
        }
      }
      //DCAM::makePointCloud(imageRangeR_vp_raw, pointcloud);
      vpImageConvert::createDepthHistogram(imageRangeR_vp_raw, imageRangeR_vp);

      DCAM::makePointCloud(imageRangeR_cv, pointcloud);
      vpDisplay::display(imageRangeR_vp);
    }

    vpHomogeneousMatrix cLThole, cRThole;
    if (TRACK_METHOD == 0){

      double ransacErrorL = 0.0;
      double elapsedTimeL = 0.0;
      double ransacErrorR = 0.0;
      double elapsedTimeR = 0.0;

      monoTrackerL->monoTrack(&imageL_vp, &cLThole, &ransacErrorL, &elapsedTimeL);
      monoTrackerR->monoTrack(&imageR_vp, &cRThole, &ransacErrorR, &elapsedTimeR);

      monoTrackerL->display(&imageL_vp);
      monoTrackerR->display(&imageR_vp);


    } else{    

      //update map of img
      mapOfImages.at(cameraNames.at(0)) = &imageL_vp;

      if (!USE_DEPTH){
        mapOfImages.at(cameraNames.at(1)) = &imageR_vp;
        stereoTracker->stereoTrack(mapOfImages, &mapOfcameraToObj);
      } else {

        // the image for the second (range) camera is not needed. Instead we
        // provide the pointcloud
        std::map<std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr> mapOfPointclouds;
        mapOfPointclouds[cameraNames.at(1)] = pointcloud;
        stereoTracker->stereoTrack(mapOfImages, mapOfPointclouds, &mapOfcameraToObj);
      }
      cLThole = mapOfcameraToObj.at(cameraNames.at(0));

      cRThole = mapOfcameraToObj.at(cameraNames.at(1));

      stereoTracker->display(mapOfImages);
    }



    vpDisplay::displayFrame(imageL_vp, cLThole, cam_left, 0.25, vpColor::none, 2);
    if (!USE_DEPTH){
      vpDisplay::displayFrame(imageR_vp, cRThole, cam_right, 0.25, vpColor::none, 2);
    } else {
      vpDisplay::displayFrame(imageRangeR_vp, cRThole, cam_right, 0.25, vpColor::none, 2);
    }

    // store estimation in struct, TODO mean tra right e left for monoTracker?
    Eigen::Matrix4d wTh_estimated_left =
        robVisInfo.robotState.wTv_eigen *
        robVisInfo.robotStruct.vTcameraL *
        CONV::matrix_visp2eigen(cLThole);
    Eigen::Matrix4d wTh_estimated_right =
        robVisInfo.robotState.wTv_eigen *
        robVisInfo.robotStruct.vTcameraR *
        CONV::matrix_visp2eigen(cRThole);



    robVisInfo.transforms.wTh_estimated_eigen = wTh_estimated_left;

    std::cout << "PUBLISHING:\n" << robVisInfo.transforms.wTh_estimated_eigen <<"\n";
    visionInterface.publishHoleTransform(robVisInfo.transforms.wTh_estimated_eigen);


    vpDisplay::displayText(imageL_vp, 30, 10, "A click to exit.", vpColor::red);
    vpDisplay::flush(imageL_vp);
    vpDisplay::flush(imageR_vp);
    if (USE_DEPTH){
      vpDisplay::flush(imageRangeR_vp);
    }
    //vpDisplay::getKeyboardEvent(imageL_vp, 'd');
    if (vpDisplay::getClick(imageL_vp, false)) {
      return 1;
    }


    if (pathLog.size() != 0){
      worldInterface.getwT(&(robVisInfo.transforms.wTh_eigen), holeName);
      CMAT::TransfMatrix wThole_cmat =
          CONV::matrix_eigen2cmat(robVisInfo.transforms.wTh_eigen);

      if (TRACK_METHOD == 0){

        logger.logCartError(robVisInfo.transforms.wTh_eigen,
                               wTh_estimated_left, "errorMonoL");
        logger.logCartError(robVisInfo.transforms.wTh_eigen,
                               wTh_estimated_right, "errorMonoR");

      } else{

        logger.logCartError(robVisInfo.transforms.wTh_eigen,
                            wTh_estimated_left, "errorStereoL");
        logger.logCartError(robVisInfo.transforms.wTh_eigen,
                            wTh_estimated_right, "errorStereoR");
      }
    } //END LOGGING


    ros::spinOnce();
    loop_rate.sleep();
  }


  if (TRACK_METHOD == 0){
    delete monoTrackerL;
    delete monoTrackerR;
  } else {
    delete stereoTracker;

  }

  return 0;
}




void append2Dto3Dfile(std::vector<cv::Point> found4CornersL, std::vector<cv::Point> found4CornersR,
                      std::string sourcePath){

  // copy original files, if destination exist, it will be overwrite (that is what we want)
  std::ifstream  srcL(sourcePath+"/vision/"+initFileClick+"left.init", std::ios::binary);
  std::ofstream  dstL(sourcePath+"/vision/"+initFile_w2D+"left.init",  std::ios::binary);
  dstL << srcL.rdbuf();
  srcL.close();

  std::ifstream  srcR(sourcePath+"/vision/"+initFileClick+"right.init", std::ios::binary);
  std::ofstream  dstR(sourcePath+"/vision/"+initFile_w2D+"right.init",   std::ios::binary);
  dstR << srcR.rdbuf();
  srcR.close();

  //write on the new file
  dstL << "#Generate by code: append 2D points" << std::endl
       << "4" << std::endl // number of points
       << found4CornersL.at(0).y << " " << found4CornersL.at(0).x << std::endl
       << found4CornersL.at(1).y << " " << found4CornersL.at(1).x << std::endl
       << found4CornersL.at(2).y << " " << found4CornersL.at(2).x << std::endl
       << found4CornersL.at(3).y << " " << found4CornersL.at(3).x << std::endl ;
  dstL.close();
  dstR << "#Generate by code: append 2D points" << std::endl
       << "4" << std::endl // number of points
       << found4CornersR.at(0).y << " " << found4CornersR.at(0).x << std::endl
       << found4CornersR.at(1).y << " " << found4CornersR.at(1).x << std::endl
       << found4CornersR.at(2).y << " " << found4CornersR.at(2).x << std::endl
       << found4CornersR.at(3).y << " " << found4CornersR.at(3).x << std::endl ;
  dstR.close();

  return;
}

void append2Dto3Dfile(std::vector<cv::Point> found4CornersL, std::string sourcePath){

  // copy original files, if destination exist, it will be overwrite (that is what we want)
  std::ifstream  srcL(sourcePath+"/vision/"+initFileClick+"left.init", std::ios::binary);
  std::ofstream  dstL(sourcePath+"/vision/"+initFile_w2D+"left.init",  std::ios::binary);
  dstL << srcL.rdbuf();
  srcL.close();

  //write on the new file
  dstL << "#Generate by code: append 2D points" << std::endl
       << "4" << std::endl // number of points
       << found4CornersL.at(0).y << " " << found4CornersL.at(0).x << std::endl
       << found4CornersL.at(1).y << " " << found4CornersL.at(1).x << std::endl
       << found4CornersL.at(2).y << " " << found4CornersL.at(2).x << std::endl
       << found4CornersL.at(3).y << " " << found4CornersL.at(3).x << std::endl ;
  dstL.close();

  return;
}