$search

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 <cv_bridge/CvBridge.h>

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

#define MAX_CAMS                                16
#define DEFAULT_MAX_PATTERNS    100
#define DEFAULT_MAX_FRAMES              200
#define DEFAULT_SET_SIZE                10
#define DEFAULT_GRID_SIZE               10
#define DEFAULT_X_COUNT                 12
#define DEFAULT_Y_COUNT                 8

#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__[] = "THERMAL_CALIBRATOR";

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

struct calibratorData {
        
        string video_stream, video_sequence;
        
        string patternType, optMethod;
        
        double candidatePatternSampleRate;      // Proportion of captured patterns to consider as candidates for model
        double testPatternSampleRate;           // Proportion of captured patterns to use to test model accuracy
        
        bool trackingMode;
        
        double alpha;
        bool autoAlpha;
        
        string outputFolder;
        
        double maxTimeDiff;
        
        double flowThreshold;
        //HGH
        double errorThreshold;

        
        double maxFrac;
        
        string read_addr;
        
        bool debugMode;
        
        string calibType;
        
        bool wantsIntrinsics;
        
        Mat K1, K2;
        Mat distCoeffs1, distCoeffs2;
        
        int maxPatterns;
        int maxFrames;
        int setSize;
        double gridSize;
        int xCount;
        int yCount;
        
        int pattType;
        int optimizationMethod;
        
        bool undistortImages;
        bool rectifyImages;
        
        bool obtainStartingData(ros::NodeHandle& nh);

        //HGH
        bool useDefinedTopics;
        string video_streamLeft, video_sequenceLeft, video_streamRight, video_sequenceRight;

        bool useIntrinsicsFile;
        string intrinsicsFileLeft;
        string intrinsicsFileRight;

        //HGH
        bool stopCapturing;
        string patternDetectionMode;
        int usePatternDetectionMode;
        bool doOnlineProcessing;
        bool useMutex;

        //HGH
        double contrastLeft;
        double brightnessLeft;
        double contrastRight;
        double brightnessRight;
        bool normLeft, normRight;
        bool invertLeft, invertRight;

        //HGH
        bool adjustMSERLeft;
        bool adjustMSERRight;
        int deltaLeft, deltaRight;
        double maxVarLeft, maxVarRight, minDivLeft, minDivRight;
        double areaThresholdLeft, areaThresholdRight;
        
};

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 prevMat1, prevMat2;
        //HGH
        Mat prevMat;
        Mat grayMat1, grayMat2;

        struct timeval cycle_timer;
        double elapsedTime;
        double avgTime;
        
        Mat map1[MAX_CAMS], map2[MAX_CAMS];
        int topValidHeight, botValidHeight, leftValid[2], rightValid[2];

        ros::Timer timer;
        
        vector<Mat> displayImages_1, displayImages_2;
        
        vector<ros::Time> times_1, times_2;
        
        vector<unsigned int> validPairs[2];
        
        unsigned int numCams;
        
        bool infoProcessed, infoProcessed_1, infoProcessed_2;
        bool stillCollecting;

        //HGH
        bool stillCollectingSet[2];

        double alpha;
        
        Mat default_R;
        
        Mat cameraMatrices[MAX_CAMS];
        Mat distCoeffVecs[MAX_CAMS];
        
        Rect roi[2];
        vector<Point2f> rectangleBounds, newRecBounds;
        Mat rectCamMat[2];
        vector<Point2f> leftLinePoints, rightLinePoints;
        
        Size imSize[MAX_CAMS];
        Mat newCamMat[MAX_CAMS];
        
        double reprojectionError_intrinsics[MAX_CAMS];
        double extendedReprojectionError_intrinsics[MAX_CAMS];
        
        vector<unsigned char> duplicateFlags_1, duplicateFlags_2;
        
        vector<int> subselectedTags_intrinsics[MAX_CAMS];
        
        string topic, topic_1, topic_2;
        string topic_info, topic_info_1, topic_info_2;
        
        int publishCount;
        
        // Extrinsics:
        Mat R_[2], P_[2];
        Mat E[2], F[2], Q1, Q2;      // Between first camera and all other cameras
        Mat R[2], Rv[2], T[2];  // Rotations/translations between first camera and all other cameras
        Mat R2[2], T2[2];       // Rotations/translations between all other cameras
        
        double stereoError;
        double extendedExtrinsicReprojectionError;
        
        bool alphaChanged;

        calibratorData configData;
        
        sensor_msgs::CameraInfo debug_camera_info_1, debug_camera_info_2;

        image_transport::CameraPublisher debug_pub_1, debug_pub_2;
        
        image_transport::CameraSubscriber camera_sub, camera_sub_1, camera_sub_2;
        
        //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, cv_ptr_1, cv_ptr_2;
        
        char debug_pub_name[256], debug_pub_name_1[256], debug_pub_name_2[256];
        
        sensor_msgs::Image msg_debug_1, msg_debug_2;
        
        ros::NodeHandle private_node_handle;
        
        char nodeName[256];
        
        Mat newImage, lastImage;
        
        unsigned int checkIndex_1, checkIndex_2;
        int frameCount, undistortionCount, rectificationCount, frameCount_1, frameCount_2;
        //HGH
        int totalFrameCount_1, totalFrameCount_2;
        
        cv::vector<Point3f> row;
    
        vector<vector<Point2f> > pointSets[MAX_CAMS], candidateSets[MAX_CAMS], testingSets[MAX_CAMS];
        vector<vector<Point2f> > extrinsicsPointSets[MAX_CAMS], extrinsicCandidateSets[MAX_CAMS], extrinsicTestingSets[MAX_CAMS];

        //HGH
        Mat lastMat[MAX_CAMS];
        vector<Point2f> cornerSets[MAX_CAMS];
        vector<int> frameCounts[MAX_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_1, cornerSet_2;
        //HGH
        vector<Point2f> cornerSet;

        //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);
        void handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
        void handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);

        
        void assignDebugCameraInfo();
        
        void imageCallback(const sensor_msgs::ImageConstPtr& msg);
        void parameterCallback(const sensor_msgs::CameraInfo& 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 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 useMode, const int cameraNumber);
        
        void updateIntrinsicMap(unsigned int idx); 
        void set_ready_for_output();
        
        void prepareForTermination();

        //HGH
        void trackPattern(int cameraNumber, const Mat& currentImg, const int currentFrame);
        bool isVerifying();



};

boost::shared_ptr < calibratorNode > *globalNodePtr;

#endif

---

thermalvis
Author(s): Stephen Vidas
autogenerated on Tue Mar 5 12:25:24 2013