calibrator.hpp

Go to the documentation of this file.

#ifndef _THERMALVIS_CALIBRATOR_H_
#define _THERMALVIS_CALIBRATOR_H_
        
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"

#include <signal.h>

#include "improc.hpp"
#include "video.hpp"
#include "calibration.hpp"
#include "intrinsics.hpp"
#include "extrinsics.hpp"
#include "features.hpp"



#include <ros/ros.h>
#include <boost/thread/thread.hpp>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>


#include <dynamic_reconfigure/server.h>
#include "calibratorConfig.h"

#define MAX_ALLOWABLE_CAMS              2
#define DEFAULT_SET_SIZE                10
#define DEFAULT_GRID_SIZE               10 // (mm)
#define DEFAULT_X_COUNT                 12
#define DEFAULT_Y_COUNT                 8
#define DEFAULT_TIMER_PERIOD    0.05

#define DEFAULT_MAX_TIME_DIFF   0.001

//HGH
#define PATTERN_CODE_INVALID -1
#define PATTERN_CODE_FIND_TRACK 0
#define PATTERN_CODE_TRACK_FIND 1
#define PATTERN_CODE_FIND 2

const char __PROGRAM__[] = "thermalvis-calibrator";

bool wantsToShutdown = false;
void mySigintHandler(int sig);

struct calibratorData {
        
        // Main inputs
        string read_addr;
        string video_stream, video_stream_secondary;
        string intrinsicsFiles[MAX_ALLOWABLE_CAMS];
                
        // Critical settings
        bool invert[MAX_ALLOWABLE_CAMS];
        string patternType, optMethod;
        double autoTimeout;
        bool trackingMode;
        string calibType;
        bool wantsIntrinsics;
        int pattType;
        int optimizationMethod;
        bool stopCapturing;
        string patternDetectionMode;
        bool useMutex;
        unsigned int numCams;
        
        bool fixPrincipalPoint;
        
        bool fixIntrinsics;
        
        bool noConstraints;
        
        bool ignoreDistortion, useRationalModel;
        
        double maxInterpolationTimegap, maxInterpolationMotion;
        
        // Course settings
        bool useUndistortedLocations;
        double alpha;
        bool autoAlpha;
        bool removeSpatialBias;
        
        // Fine settings
        double maxTimeDiff;
        int maxPatterns, maxFrames, setSize, maxCandidates, maxTests;
        double maxTime;
        double flowThreshold, errorThreshold, maxFrac;
        
        // Pattern properties
        double gridSize;
        int xCount, yCount;
        
        // Debug
        bool drawGrids;
        bool generateFigures;
        string outputFolder;
        bool debugMode;
        bool verboseMode;
        bool undistortImages;
        bool rectifyImages;
        
        // Camera models
        Mat K[MAX_ALLOWABLE_CAMS];
        Mat distCoeffs[MAX_ALLOWABLE_CAMS];
        
        // MSER settings
        bool adjustMSER[MAX_ALLOWABLE_CAMS];
        int delta[MAX_ALLOWABLE_CAMS];
        double maxVar[MAX_ALLOWABLE_CAMS], minDiv[MAX_ALLOWABLE_CAMS];
        double areaThreshold[MAX_ALLOWABLE_CAMS];
        
        // Functions
        bool obtainStartingData(ros::NodeHandle& nh);
        
};

struct validPattern {
    int  validFrameID;
    int  patternID;
    bool patternFound;
    bool isValidFrame;
};


// example: http://ua-ros-pkg.googlecode.com/svn/trunk/arrg/ua_vision/object_tracking/src/object_tracker_node.cpp
class calibratorNode {
private:

        ros::NodeHandle *ref;
        
        Mat prevMat[MAX_ALLOWABLE_CAMS], grayMat[MAX_ALLOWABLE_CAMS];

        struct timeval cycle_timer;
        double elapsedTime;
        double avgTime;
        
        struct timeval tracking_timer;
        double elapsedTrackingTime;
        double frameTrackingTime;
        
        unsigned int validSets;
        
        struct timeval vacant_timer, elapsed_timer;
        double vacantInputTime, elapsedInputTime;
        
        Mat map1[MAX_ALLOWABLE_CAMS], map2[MAX_ALLOWABLE_CAMS];
        int topValidHeight, botValidHeight, leftValid[MAX_ALLOWABLE_CAMS], rightValid[MAX_ALLOWABLE_CAMS];

        ros::Timer timer;
        
        vector<unsigned int> patternIndices[MAX_ALLOWABLE_CAMS];
        vector<Mat> displayImages[MAX_ALLOWABLE_CAMS];
        
        vector<ros::Time> times[MAX_ALLOWABLE_CAMS];
        
        vector<unsigned int> validPairs[MAX_ALLOWABLE_CAMS];
        
        
        
        bool infoProcessed[MAX_ALLOWABLE_CAMS];
        bool stillCollecting;

        double alpha;
        
        Mat default_R;
        
        Mat cameraMatrices[MAX_ALLOWABLE_CAMS];
        Mat distCoeffVecs[MAX_ALLOWABLE_CAMS];
        
        Rect roi[MAX_ALLOWABLE_CAMS];
        vector<Point2f> rectangleBounds, newRecBounds;
        Mat rectCamMat[MAX_ALLOWABLE_CAMS];
        vector<Point2f> leftLinePoints, rightLinePoints;
        
        Size imSize[MAX_ALLOWABLE_CAMS];
        Mat newCamMat[MAX_ALLOWABLE_CAMS];
        
        double reprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];
        double extendedReprojectionError_intrinsics[MAX_ALLOWABLE_CAMS];
        
        vector<unsigned char> duplicateFlags[MAX_ALLOWABLE_CAMS];
        
        vector<ros::Time> patternTimestamps[MAX_ALLOWABLE_CAMS];
        
        vector<int> subselectedTags_intrinsics[MAX_ALLOWABLE_CAMS];
        
        string topic[MAX_ALLOWABLE_CAMS];
        
        unsigned int publishCount;
        
        // Extrinsics:
        Mat R_[MAX_ALLOWABLE_CAMS], P_[MAX_ALLOWABLE_CAMS];
        Mat E[MAX_ALLOWABLE_CAMS], F[MAX_ALLOWABLE_CAMS], Q[MAX_ALLOWABLE_CAMS];      // Between first camera and all other cameras
        Mat R[MAX_ALLOWABLE_CAMS], Rv[MAX_ALLOWABLE_CAMS], T[MAX_ALLOWABLE_CAMS];  // Rotations/translations between first camera and all other cameras
        Mat R2[MAX_ALLOWABLE_CAMS], T2[MAX_ALLOWABLE_CAMS];       // Rotations/translations between all other cameras
        
        double stereoError;
        double extendedExtrinsicReprojectionError;
        
        bool alphaChanged;

        calibratorData configData;
        
        sensor_msgs::CameraInfo debug_camera_info[MAX_ALLOWABLE_CAMS];

        image_transport::CameraPublisher debug_pub[MAX_ALLOWABLE_CAMS];
        
        image_transport::CameraSubscriber camera_sub[MAX_ALLOWABLE_CAMS];
        
        //ros::Subscriber info_sub, info_sub_1, info_sub_2;
        //image_transport::Subscriber image_sub, image_sub_1, image_sub_2;
        
        cv_bridge::CvImagePtr cv_ptr[MAX_ALLOWABLE_CAMS];
        
        char debug_pub_name[MAX_ALLOWABLE_CAMS][256];
        
        sensor_msgs::Image msg_debug[MAX_ALLOWABLE_CAMS];
        
        ros::NodeHandle private_node_handle;
        
        char nodeName[256];
        
        Mat newImage, lastImage;
        
        unsigned int checkIndex[MAX_ALLOWABLE_CAMS];
        unsigned int frameCount[MAX_ALLOWABLE_CAMS], undistortionCount, rectificationCount, totalFrameCount[2];
        
        cv::vector<Point3f> row;
    
        vector<vector<Point2f> > pointSets[MAX_ALLOWABLE_CAMS], candidateSets[MAX_ALLOWABLE_CAMS], testingSets[MAX_ALLOWABLE_CAMS];
        vector<vector<Point2f> > extrinsicsPointSets[MAX_ALLOWABLE_CAMS], extrinsicCandidateSets[MAX_ALLOWABLE_CAMS], extrinsicTestingSets[MAX_ALLOWABLE_CAMS];

        //HGH
        Mat lastMat[MAX_ALLOWABLE_CAMS];
        vector<Point2f> cornerSets[MAX_ALLOWABLE_CAMS];
        vector<unsigned int> frameCounts[MAX_ALLOWABLE_CAMS];
        bool  doVerify;

                
        boost::shared_mutex _access;
        
        bool readyForOutput;
        
        dynamic_reconfigure::Server<thermalvis::calibratorConfig> server;
        dynamic_reconfigure::Server<thermalvis::calibratorConfig>::CallbackType f;
        
        vector<Point2f> cornerSet[MAX_ALLOWABLE_CAMS];

        //HGH
        void preprocessImage(Mat src, Mat &dst, double a, double b, bool normaliz, bool negative );



public:
        calibratorNode(ros::NodeHandle& nh, calibratorData startupData);
        
        void performIntrinsicCalibration();
        void performExtrinsicCalibration();
        
        void handle_image(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg, const unsigned int camera_index = 0);
        
        
        void create_virtual_pairs();
        
        void assignDebugCameraInfo();
        
        void imageCallback(const sensor_msgs::ImageConstPtr& msg);
        
        void publishUndistorted(const ros::TimerEvent& event);
        void startUndistortionPublishing();
        
        void publishRectified(const ros::TimerEvent& event);
        void startRectificationPublishing();
        
        bool isStillCollecting();
        bool wantsToUndistort();
        bool wantsToRectify();
        void writeResults();
        bool wantsIntrinsics();
        bool wantsExtrinsics();
        void updatePairs();

        //HGH
        void determineValidPairs();
        void evaluateFrames();

        void getAverageTime();
        void getFrameTrackingTime();
        void assignIntrinsics();
        
        void preparePatternSubsets();
        void prepareExtrinsicPatternSubsets();
        
        void serverCallback(thermalvis::calibratorConfig &config, uint32_t level);
        void updateMap();
        
        bool findPattern(const Mat& im, vector<Point2f>& dst, Mat& prev, const int cameraNumber);
        
        void updateIntrinsicMap(unsigned int idx); 
        void set_ready_for_output();
        
        void prepareForTermination();

        //HGH
        bool isVerifying();

        void timerCallback(const ros::TimerEvent& e);

};

boost::shared_ptr < calibratorNode > *globalNodePtr;

#endif