image_calibration.h

Go to the documentation of this file.

#include <ros/publisher.h>
#include <ros/subscriber.h>
#include <ros/node_handle.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/gpu/gpu.hpp>
#include <fstream>
#include <cstdlib>
#include <ros/ros.h>
#include <cv.h>
#include <opencv2/highgui/highgui.hpp>

#include <youbot_overhead_vision/markerCoords.h>
#include <youbot_overhead_vision/CalibCameraImg.h>
#include <youbot_overhead_vision/PositionFromCamera.h>
#include "yaml-cpp/yaml.h"
#include "calibTree.h"

using namespace std;
#define ENCODING "rgb8"
#define FRAME_ID "calibration"
#define CALIB_ACCURACY 10

//Object Types 
/*
 * L= Left  R = Right.  F = Front B = Back
 */

//Calibration Types
#define ROBOT_LEFT_FRONT 0
#define ROBOT_RIGHT_FRONT 1
#define ROBOT_LEFT_BACK 2
#define ROBOT_RIGHT_BACK 3
#define FLOOR 4
#define NOT_SET 5

//Marker Clusters Definitions
#define MIN_MARKER_PIXELS 6
#define MARKER_CLUSTER_RADIUS 10 

//Robot bounding box thresholds
#define MIN_BOUND_WIDTH 45
#define MIN_BOUND_LENGTH 77
#define MAX_BOUND_WIDTH 80
#define MAX_BOUND_LENGTH 120
#define DST_BOUND_THRESH 50

//Extensions beyond markers
#define FRONT_EXTENDED 33
#define RIGHT_EXTENDED 19
#define BACK_EXTENDED 28
#define LEFT_EXTENDED 19

//Robot Defs For simulated camera
#define SIDE_MARKER_DIST  41
#define FRONT_MARKER_DIST 36
#define DIAG_MARKER_DIST 54
#define MARKER_ACCURACY 8

//Camera inputs
#define OVERHEAD_NORTH "/logitech_9000_camera1/image_raw"
#define OVERHEAD_SOUTH "/logitech_9000_camera2/image_raw"

//overlap constants: #Number of rows of pixels that are found in both cameras.
#define SIM_OVERLAP 30
#define REAL_OVERLAP 62

#define NORTH_CAMERA 1
#define SOUTH_CAMERA 2
#define BOTH_CAMERAS 3

#define PI 3.14159

class imageCalibration
{
public:

  // Determines if the simulation or real world is currently being used.
  bool useSim;

  int lastFrameMidpointX;
  int lastFrameMidpointY;

  //Determines how close to the calibrating colors a pixel must be to be considered that color.
  int calibAccuracy;

  //ROS Publisher and Subscribers
  ros::Subscriber imgSub1;
  ros::Subscriber imgSub2;
  ros::ServiceServer service;
  ros::Publisher pubImgCal;
  ros::Publisher pubRobotPosition;

  //Contains an image representing the walls
  cv::Mat roomBoundary;

  //Contains the colors to apply if a pixel matches a certain calibration value.  Format:[CalibrationType][R,G,B]
  int colors[6][3];

  //Struct used to contain the position of marker points, separated into different clusters
  struct markerPoints
  {
    int numPoints;
    int avgX;
    int avgY;
    markerPoints *next;
    int markerType; //Indicates which marker was found.
  };

  //Pointer to the calibTree object, which allows for tree creating/parsing
  calibTree * calTree;

  //vector containing all the robot marker points clusters
  vector<markerPoints> robotMarkersVect;

  //Stores the cal image
  cv_bridge::CvImagePtr output_Img1;
  cv_bridge::CvImagePtr output_Img2;

  //raw image updated by the image_raw subscriber
  cv_bridge::CvImagePtr raw_Img1;
  cv_bridge::CvImagePtr raw_Img2;

  //marks whether or not the image still has to be updated before being analyzed
  bool updatedImageFlag;

  int blurringFactor;
  int seq_num;

  //This Mat maintains an image with all the known obstacles in the scene.  When the robot is removed by a box in the main image, this image contains what was known to be in the area before the robot entered that area
  cv::Mat pastObstImg;

  imageCalibration();

  /*
   *   Destructor
   */
  ~imageCalibration();

  void getCalibration(int calibType, int calibR, int calibB, int calibG);

  void processImage();

  void imageCallback1(const sensor_msgs::ImageConstPtr& image_msg);

  void imageCallback2(const sensor_msgs::ImageConstPtr& image_msg);

  void addClusters(vector<int> *ImageXCoords, vector<int> *ImageYCoords);

  void applyCalib();

  int count;
  bool applyCalibToPixel(int index, int calibType, cv_bridge::CvImagePtr image_msg, int camera);

  void addMarkerPoint(int pointX, int pointY, int calibType);

  bool extrapolateMarkers(vector<int> *ImageXCoords, vector<int> *ImageYCoords);

  void removeRobot(vector<int> xCoords, vector<int> yCoords, int xpos, int ypos);

  vector<cv::RotatedRect> findBoundingBox(cv::Mat image, int xpos, int ypos, int *minDstIndex);

  void applyBoundingBox(cv::Mat *image, cv::RotatedRect rect);

  void resetForNextFrame();

  string convertInt(int number);

  void freerobotVector();

private:
  //File paths for calibration files
  string filenames[5];
};