$search

calibrator.cpp

Go to the documentation of this file.

#include "calibrator.hpp"

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

        ROS_INFO("Node launched.");
        
        ros::init(argc, argv, "mm_calibrator");
        
        ros::NodeHandle private_node_handle("~");
        
        calibratorData startupData;
        
        startupData.read_addr = argv[0];
        startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-11);
                
        bool inputIsValid = startupData.obtainStartingData(private_node_handle);
        
        if (!inputIsValid) {
                ROS_ERROR("Configuration invalid.");
        }
                
        ROS_INFO("Startup data processed.");
        
        
        ros::NodeHandle nh;
        
        calibratorNode calib_node(nh, startupData);
        //boost::shared_ptr < calibratorNode > calib_node (new calibratorNode (nh, startupData));
        //globalNodePtr = &calib_node;
        
        signal(SIGINT, mySigintHandler);
        
        
        
        ROS_INFO("Node configured.");
        
        while (calib_node.isStillCollecting()) {
                ros::spinOnce();
        }
        
        ROS_INFO("Patterns captured.");
        

        if (calib_node.wantsIntrinsics()) {
                calib_node.preparePatternSubsets();
                calib_node.performIntrinsicCalibration();
                calib_node.writeResults();
                ROS_INFO("Calibration completed.");

        } else {
                calib_node.assignIntrinsics();
        }
        
        if (calib_node.wantsExtrinsics()) {
                
                ROS_INFO("Performing extrinsics...");

                //HGH
                if(!startupData.useMutex){
                    //determine valid frame pairs
                    calib_node.determineValidPairs();
                    //evaluate frame frames
                    calib_node.evaluateFrames();
                }

                while (calib_node.isVerifying()) {
                        ros::spinOnce();
                }
                
                calib_node.prepareExtrinsicPatternSubsets();
                calib_node.performExtrinsicCalibration();
                calib_node.writeResults();
        }
        
        calib_node.set_ready_for_output();
        
        if (calib_node.wantsToUndistort() || (calib_node.wantsToRectify())) {
                calib_node.getAverageTime();
        }
        
        if (calib_node.wantsToUndistort()) {
                
                if (calib_node.wantsIntrinsics()) {
                        ROS_INFO("Publishing undistorted images...");
                
                        calib_node.startUndistortionPublishing();
                        
                        while (calib_node.wantsToUndistort()) {
                                ros::spinOnce();
                        }
                        
                        ROS_INFO("Publishing complete.");
                }
        }
        
        if (calib_node.wantsToRectify()) {      
                if (calib_node.wantsExtrinsics()) {
                        
                        ROS_INFO("Publishing rectified images...");
                
                        calib_node.startRectificationPublishing();
                        
                        while (calib_node.wantsToRectify()) {
                                ros::spinOnce();
                        }
                        
                        ROS_INFO("Publishing complete.");
                        
                }


                
        }
        
        return 0;
        
}

void calibratorNode::prepareExtrinsicPatternSubsets() {

        ROS_WARN("prepareExtrinsicPatternSubsets...");

        if(!configData.useMutex){

        //HGH -  use all extrinsics point sets!
        extrinsicCandidateSets[0] = extrinsicsPointSets[0];
        extrinsicCandidateSets[1] = extrinsicsPointSets[1];
        extrinsicTestingSets[0] = extrinsicsPointSets[0];
        extrinsicTestingSets[1] = extrinsicsPointSets[1];

        }else{

        srand((unsigned)time(0));
        
        vector<vector<Point2f> > tmpSet[MAX_CAMS];
        
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                
                for (int iii = 0; iii < extrinsicsPointSets[xxx].size(); iii++) {
                        tmpSet[xxx].push_back(extrinsicsPointSets[xxx].at(iii));
                }
                
        }
        
        // While the candidate quantity is insufficient
        while (((double) extrinsicCandidateSets[0].size()) < configData.candidatePatternSampleRate*double(extrinsicsPointSets[0].size())) {
                
                unsigned int randomIndex = rand() % tmpSet[0].size();
                
                for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                        extrinsicCandidateSets[xxx].push_back(tmpSet[xxx].at(randomIndex));
                        tmpSet[xxx].erase(tmpSet[xxx].begin() + randomIndex);
                }
                
        }
        
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                tmpSet[xxx].clear();
        }
        
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                
                for (int iii = 0; iii < extrinsicsPointSets[xxx].size(); iii++) {
                        tmpSet[xxx].push_back(extrinsicsPointSets[xxx].at(iii));
                }
                
        }
        
        // While the testing quantity is insufficient
        while (((double) extrinsicTestingSets[0].size()) < configData.testPatternSampleRate*double(extrinsicsPointSets[0].size())) {
                
                unsigned int randomIndex = rand() % tmpSet[0].size();
                
                for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                        extrinsicTestingSets[xxx].push_back(tmpSet[xxx].at(randomIndex));
                        tmpSet[xxx].erase(tmpSet[xxx].begin() + randomIndex);
                }
                
        }
        //*/
        }
        
}

void calibratorNode::preparePatternSubsets() {

        ROS_WARN("preparePatternSubsetss...");
        
        srand((unsigned)time(0));

        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                vector<vector<Point2f> > tmpSet;
                
                for (int iii = 0; iii < pointSets[xxx].size(); iii++) {
                        tmpSet.push_back(pointSets[xxx].at(iii));
                }
                
                // While the candidate quantity is insufficient
                while (((double) candidateSets[xxx].size()) < configData.candidatePatternSampleRate*double(pointSets[xxx].size())) {
                        
                        unsigned int randomIndex = rand() % tmpSet.size();
                        
                        candidateSets[xxx].push_back(tmpSet.at(randomIndex));
                        
                        tmpSet.erase(tmpSet.begin() + randomIndex);
                        
                }
                
                tmpSet.clear();
                
                for (int iii = 0; iii < pointSets[xxx].size(); iii++) {
                        tmpSet.push_back(pointSets[xxx].at(iii));
                }
                
                // While the testing quantity is insufficient
                while (((double) testingSets[xxx].size()) < configData.testPatternSampleRate*double(pointSets[xxx].size())) {
                        
                        unsigned int randomIndex = rand() % tmpSet.size();
                        
                        testingSets[xxx].push_back(tmpSet.at(randomIndex));
                        
                        tmpSet.erase(tmpSet.begin() + randomIndex);
                        
                }
                
        }

}

void calibratorNode::prepareForTermination() {
        return; 
}

void mySigintHandler(int sig) {
        wantsToShutdown = true;
        ROS_WARN("Requested shutdown... terminating feeds...");
        
        (*globalNodePtr)->prepareForTermination();
}

void calibratorNode::assignIntrinsics() {
        
        configData.K1.copyTo(cameraMatrices[0]);
        configData.K2.copyTo(cameraMatrices[1]);
        configData.distCoeffs1.copyTo(distCoeffVecs[0]);
        configData.distCoeffs2.copyTo(distCoeffVecs[1]);

}

bool calibratorNode::wantsIntrinsics() {
        if (configData.calibType == "single") {
                return true;
        } else {
                if (configData.wantsIntrinsics) {
                        return true;
                } else {
                        return false;
                }
        }
}

bool calibratorNode::wantsExtrinsics() {
        if (configData.calibType == "stereo") {
                return true;
        } else {
                return false;
        }
}

void calibratorNode::writeResults() {

        
        if (wantsIntrinsics()) {
                for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                        
                        updateIntrinsicMap(xxx);
                        
                        stringstream ss;
                        ss << (xxx+1);
                        string outputName = configData.outputFolder + "/" + "intrinsics-" + ss.str() + ".yml";
                        
                        FileStorage fs(outputName, FileStorage::WRITE);
                        
                        Mat imageSize(1, 2, CV_16UC1);
                        imageSize.at<unsigned short>(0,0) = imSize[xxx].width;
                        imageSize.at<unsigned short>(0,1) = imSize[xxx].height;
                        fs << "imageSize" << imageSize;
                        fs << "cameraMatrix" << cameraMatrices[xxx];
                        fs << "distCoeffs" << distCoeffVecs[xxx];
                        fs << "newCamMat" << newCamMat[xxx];
                        fs << "alpha" << configData.alpha;

                        fs << "reprojectionError" << reprojectionError_intrinsics[xxx];
                        fs << "generalisedError" << extendedReprojectionError_intrinsics[xxx];

                        fs << "patternsUsed" << ((int) subselectedTags_intrinsics[xxx].size());

                        fs.release();

                         ROS_WARN("Wrote intrinsics calibration data to:\n %s", outputName.c_str());
                        
                }

        }
        
        if (wantsExtrinsics()) {
                
                string outputName = configData.outputFolder + "/" + "stereo-extrinsics" + ".yml";
                
                FileStorage fs(outputName, FileStorage::WRITE);
                
                string tmpString;
                char tmp[64];
                
                sprintf(tmp, "reprojectionErr");
        tmpString = string(tmp);
        fs << tmpString << stereoError;
        
                sprintf(tmp, "generalisedMRE");
        tmpString = string(tmp);
        fs << tmpString << extendedExtrinsicReprojectionError;

                
                for (int i = 0; i < numCams; i++) {

            sprintf(tmp, "R%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << R[i];

            sprintf(tmp, "Rvec%d", i);
            tmpString = string(tmp);
            fs << tmpString << Rv[i];

            sprintf(tmp, "T%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << T[i];

            sprintf(tmp, "R_%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << R_[i];

            sprintf(tmp, "P_%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << P_[i];

            sprintf(tmp, "cameraMatrix%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << cameraMatrices[i];

            sprintf(tmp, "distCoeffs%d", i);
            //printf("%s << tmp = %s.\n", __FUNCTION__, tmp);
            tmpString = string(tmp);
            fs << tmpString << distCoeffVecs[i];
        }
        
        fs << "E" << E[1];
        fs << "F" << F[1];

        //HGH
         fs << "R" << R[1];
         fs << "T" << T[1];
         fs << "alpha" << configData.alpha;

         fs << "Q1" << Q1;
         fs << "Q2" << Q2;


                
        fs.release();

          ROS_WARN("Wrote extrinsic calibration data to:\n %s", outputName.c_str());
                        
        }
        
        
}

bool calibratorNode::wantsToUndistort() {
        return configData.undistortImages;
}

bool calibratorNode::wantsToRectify() {
        return configData.rectifyImages;
}

void calibratorNode::getAverageTime() {
        
        int divisorFrames = max(frameCount, frameCount_1);
        avgTime /= ((double) divisorFrames - 1);
        ROS_INFO("Average time per frame: (%f)", avgTime);
        
}

void calibratorNode::startUndistortionPublishing() {
        
        
        //Rect* validPixROI = 0;
        
        //bool centerPrincipalPoint = true;
        
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                updateIntrinsicMap(xxx);
                //newCamMat[xxx] = getOptimalNewCameraMatrix(cameraMatrices[xxx], distCoeffVecs[xxx], imSize[xxx], configData.alpha, imSize[xxx], validPixROI, centerPrincipalPoint);
                //initUndistortRectifyMap(cameraMatrices[xxx], distCoeffVecs[xxx], default_R, newCamMat[xxx], imSize[xxx], CV_32FC1, map1[xxx], map2[xxx]);
        }
        
        
        // Set up timer
        timer = ref->createTimer(ros::Duration(avgTime / 1000.0), &calibratorNode::publishUndistorted, this);
        
}

void calibratorNode::startRectificationPublishing() {

        
        Rect* validPixROI = 0;


        bool centerPrincipalPoint = true;
        
        Point pt1, pt2;
        vector<Point2f> rectangleBounds, newRecBounds;

        

        Mat blankCoeffs(1, 8, CV_64F);
        
        /*
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                newCamMat[xxx] = getOptimalNewCameraMatrix(cameraMatrices[xxx], distCoeffVecs[xxx], imSize[xxx], alpha, imSize[xxx], validPixROI, centerPrincipalPoint);
                initUndistortRectifyMap(cameraMatrices[xxx], distCoeffVecs[xxx], R[xxx], newCamMat[xxx], imSize[xxx], CV_32FC1, map1[xxx], map2[xxx]);
        }
        */
        
        for (int i = 0; i < 2; i++) {

                ROS_INFO("Initializing rectification map (%d)...", i);

                initUndistortRectifyMap(cameraMatrices[i],
                                                                distCoeffVecs[i],
                                                                R_[i],
                                                                P_[i],  // newCamMat[i]
                                                                imSize[i],
                                                                CV_32F,     // CV_16SC2
                                                                map1[i],    // map1[i]
                                                                map2[i]);   // map2[i]


                ROS_INFO("Map (%d) initialized.", i);

                pt1 = Point(roi[i].x, roi[i].y);
                pt2 = Point(roi[i].x + roi[i].width, roi[i].y + roi[i].height);

                if ((pt1.x == 0) && (pt1.y == 0) && (pt2.x == 0) && (pt2.y == 0)) {
                        ROS_WARN("ROI is defined as having zero area...");
                } else {
                        printf("%s << (und) pt1 = (%d, %d); pt2 = (%d, %d)\n", __FUNCTION__, pt1.x, pt1.y, pt2.x, pt2.y);

                        rectangleBounds.push_back(Point2f(pt1.x, pt1.y));
                        rectangleBounds.push_back(Point2f(pt2.x, pt2.y));
                        
                        Rect validROI_x;
                        //HGH
                        //rectCamMat[i] = getOptimalNewCameraMatrix(cameraMatrices[i], distCoeffVecs[i], imSize[i], 0.5, imSize[i], &validROI_x);
                        rectCamMat[i] = getOptimalNewCameraMatrix(cameraMatrices[i], distCoeffVecs[i], imSize[i], configData.alpha, imSize[i], &validROI_x);




                        /*
                        undistortPoints(Mat(rectangleBounds), newRecBounds, rectCamMat[i], blankCoeffs, R_[i], P_[i]);

                        //printf("%s << Original rectangle points: = (%d, %d) & (%d, %d)\n", __FUNCTION__, pt1.x, pt1.y, pt2.x, pt2.y);

                        pt1 = Point(int(newRecBounds.at(0).x), int(newRecBounds.at(0).y));
                        pt2 = Point(int(newRecBounds.at(1).x), int(newRecBounds.at(1).y));

                        printf("%s << pt1 = (%d, %d); pt2 = (%d, %d)\n", __FUNCTION__, pt1.x, pt1.y, pt2.x, pt2.y);

                        if (pt1.y > topValidHeight)
                        {
                                topValidHeight = pt1.y;
                        }

                        if (pt2.y < botValidHeight)
                        {
                                botValidHeight = pt2.y;
                        }

                        leftValid[i] = pt1.x;
                        rightValid[i] = pt2.x;
                        */
                }

                
        }

        printf("%s << topValidHeight = %d; botValidHeight = %d\n", __FUNCTION__, topValidHeight, botValidHeight);

        

        // Prepare epipolar lines etc:
        for (int k = 1; k < 32; k++)
        {
                // should try to center it on the final (thermal) image
                leftLinePoints.push_back(Point2f(0, k*(botValidHeight - topValidHeight)/32 - 1));
                rightLinePoints.push_back(Point2f(imSize[0].width, k*(botValidHeight - topValidHeight)/32 - 1));
        }
        
        
        
        
        // Set up timer
        timer = ref->createTimer(ros::Duration(avgTime / 1000.0), &calibratorNode::publishRectified, this);
        
}

void calibratorNode::publishUndistorted(const ros::TimerEvent& event) {
        
        //ROS_INFO("Entered timed loop...");
        
        Mat dispMat;
        
        if (configData.calibType == "single") {
                if (undistortionCount >= frameCount) {
                        configData.undistortImages = false;
                        return;
                }
                
                // Draw grid
                
                Mat preMat, postMat;
                drawGrid(displayImages_1.at(undistortionCount), preMat, 0);
                
                //ROS_WARN("About to publish undistorted frames...");
                remap(preMat, postMat, map1[0], map2[0], INTER_LINEAR, BORDER_TRANSPARENT);
                
                drawGrid(postMat, dispMat, 1);
                
                std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_1.data.begin());
                debug_pub_1.publish(msg_debug_1, debug_camera_info_1);
                //ROS_WARN("Message 1 published...");
                
        } else {
                if (undistortionCount >= validPairs[0].size()) {
                        configData.undistortImages = false;
                        return;
                }
                
                Mat preMat, postMat;
                
                //ROS_WARN("About to publish undistorted frames...");
                drawGrid(displayImages_1.at(validPairs[0].at(undistortionCount)), preMat, 0);
                remap(preMat, postMat, map1[0], map2[0], INTER_LINEAR, BORDER_TRANSPARENT);
                drawGrid(postMat, dispMat, 1);
                std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_1.data.begin());
                debug_pub_1.publish(msg_debug_1, debug_camera_info_1);
                //ROS_WARN("Message 1 published...");
                
                if (numCams > 1) {
                        drawGrid(displayImages_2.at(validPairs[1].at(undistortionCount)), preMat, 0);
                        remap(preMat, postMat, map1[1], map2[1], INTER_LINEAR, BORDER_TRANSPARENT);
                        drawGrid(postMat, dispMat, 1);
                        std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_2.data.begin());
                        debug_pub_2.publish(msg_debug_2, debug_camera_info_2);          
                }
        }
        
        undistortionCount++;
}

void calibratorNode::publishRectified(const ros::TimerEvent& event) {
        
        //ROS_INFO("Entered timed (rectification) loop...");
        
        Mat dispMat;
        
        
        if (rectificationCount >= validPairs[0].size()) {
                configData.rectifyImages = false;
                return;
        }
        
        Mat und1, und2, disp1, disp2;
        
        //ROS_WARN("Publishing rectified images...");
        remap(displayImages_1.at(validPairs[0].at(rectificationCount)), und1, map1[0], map2[0], INTER_LINEAR);
        //HGH
        //disp1 = displayImages_1.at(validPairs[0].at(rectificationCount));
        //und1 = displayImages_1.at(validPairs[0].at(rectificationCount));
        drawGrid(und1, disp1, 1);

        //HGH - draw roi
        rectangle(disp1, Point2f(roi[0].x, roi[0].y), Point2f(roi[0].x + roi[0].width , roi[0].y + roi[0].height), CV_RGB(127, 255, 0));

        
        std::copy(&(disp1.at<Vec3b>(0,0)[0]), &(disp1.at<Vec3b>(0,0)[0])+(disp1.cols*disp1.rows*disp1.channels()), msg_debug_1.data.begin());
        debug_pub_1.publish(msg_debug_1, debug_camera_info_1);
        
        remap(displayImages_2.at(validPairs[1].at(rectificationCount)), und2, map1[1], map2[1], INTER_LINEAR);
        drawGrid(und2, disp2, 1);
        //HGH - draw roi
        rectangle(disp2, Point2f(roi[1].x, roi[1].y), Point2f(roi[1].x + roi[1].width , roi[1].y + roi[1].height), CV_RGB(127, 255, 0));

        std::copy(&(disp2.at<Vec3b>(0,0)[0]), &(disp2.at<Vec3b>(0,0)[0])+(disp2.cols*disp2.rows*disp2.channels()), msg_debug_2.data.begin());
        debug_pub_2.publish(msg_debug_2, debug_camera_info_2);
        
        rectificationCount++;
}

void calibratorNode::performExtrinsicCalibration() {

        ROS_WARN("preparePatternSubsets...");
        
        ROS_INFO("Total of (%d) patterns found from (%d) valid frame pairs.", extrinsicsPointSets[0].size(), validPairs[0].size());
        
        ROS_INFO("Initial intrinsics: ");
        
        cout << "K1 = " << configData.K1 << endl;
        
        cout << "K2 = " << configData.K2 << endl;
        
        cout << "imSize[0] = (" << imSize[0].height << ", " << imSize[0].width << ")" << endl;
        
        cv::vector<cv::vector<Point2f> > emptyPointSetVector;
        vector<int> emptyIntVector;

        vector<vector<int> > extrinsicTagNames, extrinsicSelectedTags;
        cv::vector<cv::vector<cv::vector<Point2f> > > extrinsicsList, extrinsicsCandidates;

        //HGH
        //      vector<string> extractedList;
        //      cv::vector<cv::vector<Point2f> > cornersList[2];
        
        cv::vector<Mat> extrinsicsDistributionMap;
        extrinsicsDistributionMap.resize(2);

        for (unsigned int nnn = 0; nnn < 2; nnn++) {
                extrinsicsList.push_back(emptyPointSetVector);
                extrinsicsCandidates.push_back(emptyPointSetVector);
                extrinsicTagNames.push_back(emptyIntVector);

                extrinsicsDistributionMap.at(nnn) = Mat(imSize[nnn], CV_8UC1);
        }

        
        for (unsigned int iii = 0; iii < extrinsicsPointSets[0].size(); iii++) {

                for (unsigned int nnn = 0; nnn < 2; nnn++) {
                        extrinsicsList.at(nnn).push_back(extrinsicsPointSets[nnn].at(iii));
                        extrinsicsCandidates.at(nnn).push_back(extrinsicsPointSets[nnn].at(iii));
                        extrinsicTagNames.at(nnn).push_back(iii);
                }
        }
        
        vector<Size> extrinsicsSizes;
        extrinsicsSizes.push_back(imSize[0]);
        extrinsicsSizes.push_back(imSize[1]);

        //HGH - use all available patterns
        //-> configData.optimizationMethod = allPatterns
        optimizeCalibrationSets(extrinsicsSizes, 2, cameraMatrices, distCoeffVecs, extrinsicsDistributionMap, extrinsicsCandidates, extrinsicsList, row, configData.optimizationMethod, configData.setSize, extrinsicTagNames, extrinsicSelectedTags);


        ROS_WARN("extrinsicsList.size() = %d", extrinsicsList.at(0).size());
        ROS_WARN("extrinsicsCandidates.size() = %d", extrinsicsCandidates.at(0).size());
        
        // SO CANDIDATES IS WHERE THE SUBSELECTION ARE KEPT..
        
        TermCriteria term_crit;
        term_crit = TermCriteria(TermCriteria::COUNT+ TermCriteria::EPS, 30, 1e-6);


        // Should make R[0] and T[0] the "identity" matrices
        R[0] = Mat::eye(3, 3, CV_64FC1);
        T[0] = Mat::zeros(3, 1, CV_64FC1);
        // HGH - R[0] and T[0] not used but instead R[1] and T[1]
        R[1] = Mat::eye(3, 3, CV_64FC1);
        T[1] = Mat::zeros(3, 1, CV_64FC1);

        cv::vector< cv::vector<Point3f> > objectPoints;


        //HGH
//      for (int iii = 0; iii < extrinsicsCandidates[0].size(); iii++)
//      {
//              objectPoints.push_back(row);
//      }
        //HGH -or version from above can be used...
        objectPoints.resize(extrinsicsCandidates[0].size());
        for (int iii = 0; iii < extrinsicsCandidates[0].size(); iii++)
        {
            for (int j = 0; j < configData.yCount; j++ ){
                for (int i = 0; i < configData.xCount; i++ ){
                    objectPoints[iii].push_back(Point3f(j*configData.gridSize, i*configData.gridSize,0));
                }
            }
        }


        stereoError = stereoCalibrate(objectPoints,
                                        extrinsicsCandidates.at(0), extrinsicsCandidates.at(1),
                                        cameraMatrices[0], distCoeffVecs[0],
                                        cameraMatrices[1], distCoeffVecs[1],
                                        extrinsicsSizes[0],                      // hopefully multiple cameras allow multiple image sizes
                                        R[1], T[1],
                                        E[1], F[1],
                                        term_crit,
                                        CV_CALIB_FIX_INTRINSIC); //CV_CALIB_RATIONAL_MODEL +

                        
        cout << "cameraMatrices[0] = " << cameraMatrices[0] << endl;
        cout << "cameraMatrices[1] = " << cameraMatrices[1] << endl;
        cout << "distCoeffVecs[0] = " << distCoeffVecs[0] << endl;
        cout << "distCoeffVecs[1] = " << distCoeffVecs[1] << endl;
        cout << "R[1] = " << R[1] << endl;
        cout << "T[1] = " << T[1] << endl;
        printf("extrinsicsSizes[0] = (%d, %d)\n", extrinsicsSizes[0].width, extrinsicsSizes[0].height);

        //alpha = 1.00;

        //HGH - use optimal alpha based on left camera if using auto alpha
        if (configData.autoAlpha) {
                configData.alpha = findBestAlpha(cameraMatrices[0], distCoeffVecs[0], imSize[0]);
                ROS_INFO("Optimal alpha = (%f)", configData.alpha);
        }

        cout << "alpha = " << configData.alpha << endl;

        //HGH
        stereoRectify(cameraMatrices[0], distCoeffVecs[0], cameraMatrices[1], distCoeffVecs[1],
                                  imSize[0],
                                  R[1], T[1],
                                  R_[0], R_[1], P_[0], P_[1],
                                  Q1,
                                  CALIB_ZERO_DISPARITY, /* or 0 */
                                  configData.alpha, imSize[0], &roi[0], &roi[1]);
//        stereoRectify(cameraMatrices[0], distCoeffVecs[0], cameraMatrices[1], distCoeffVecs[1],
//                                  imSize[0],
//                                  R[1], T[1],
//                                  R_[0], R_[1], P_[0], P_[1],
//                                  Q1,
//                                  CALIB_ZERO_DISPARITY,
//                                  configData.alpha, imSize[0], &roi[0], &roi[1]);

/* HGH - this part was obviously to debug something?!!!
        Rect dummy_roi[2];
        stereoRectify(cameraMatrices[1], distCoeffVecs[1], cameraMatrices[0], distCoeffVecs[0],
                                  imSize[1],
                                  R[1].inv(), -T[1],
                                  R_[1], R_[0], P_[1], P_[0],
                                  Q2,
                                  CALIB_ZERO_DISPARITY,
                                  configData.alpha, imSize[1], &dummy_roi[0], &dummy_roi[1]);
//*/


        printf("imsize[0] = (%d, %d)\n", imSize[0].width, imSize[0].height);
        printf("imsize[1] = (%d, %d)\n", imSize[1].width, imSize[1].height);

                                  
        cout << "R_[0] = " << R_[0] << endl;
        cout << "R_[1] = " << R_[1] << endl;
        cout << "P_[0] = " << P_[0] << endl;
        cout << "P_[1] = " << P_[1] << endl;
        cout << "Q1 = " << Q1 << endl;
        cout << "Q2 = " << Q2 << endl;
                                  
        printf("roi[0] = (%d, %d, %d, %d)\n", roi[0].x, roi[0].y, roi[0].width, roi[0].height);
        printf("roi[1] = (%d, %d, %d, %d)\n", roi[1].x, roi[1].y, roi[1].width, roi[1].height);
        
        extendedExtrinsicReprojectionError = calculateExtrinsicERE(2, objectPoints.at(0), extrinsicsList, cameraMatrices, distCoeffVecs, R, T);
                                        
        cout << "R = " << R[1] << endl;
        cout << "T = " << T[1] << endl;
        
        return;
        
}

void calibratorNode::performIntrinsicCalibration() {
        
        #pragma parallel for
        for (unsigned int xxx = 0; xxx < numCams; xxx++) {
                
                ROS_INFO("Intrinsically calibrating camera (%d) with (%d / %d / %d) patterns...", xxx, candidateSets[xxx].size(), testingSets[xxx].size(), pointSets[xxx].size());
                
                optimizeCalibrationSet(imSize[xxx], candidateSets[xxx], testingSets[xxx], row, subselectedTags_intrinsics[xxx], configData.optimizationMethod, configData.setSize, true);
                
                ROS_INFO("Set optimized (%d)", subselectedTags_intrinsics[xxx].size());
                
                vector< vector<Point3f> > objectPoints;
                vector< vector<Point2f> > subselectedPatterns;
                vector<Mat> rvecs, tvecs;

                for (int iii = 0; iii < subselectedTags_intrinsics[xxx].size(); iii++) {
                        objectPoints.push_back(row);
                        subselectedPatterns.push_back(candidateSets[xxx].at(iii));
                }

                rvecs.resize(subselectedTags_intrinsics[xxx].size());
                tvecs.resize(subselectedTags_intrinsics[xxx].size());
                
                Mat camMat, distCoeffs;
                
                // Restrictive: CV_CALIB_FIX_K5 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K2 + CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_FIX_ASPECT_RATIO + CV_CALIB_ZERO_TANGENT_DIST
                // Rational: CV_CALIB_RATIONAL_MODEL
                // Conventional: 0
                int intrinsicsFlags = 0;
                
                reprojectionError_intrinsics[xxx] = calibrateCamera(objectPoints, subselectedPatterns, imSize[xxx], cameraMatrices[xxx], distCoeffVecs[xxx], rvecs, tvecs, intrinsicsFlags);
                
                ROS_INFO("Reprojection error = %f", reprojectionError_intrinsics[xxx]);
                
                double *errValues;
                errValues = new double[subselectedPatterns.size() * subselectedPatterns.at(0).size()];

                extendedReprojectionError_intrinsics[xxx] = calculateERE(imSize[xxx], objectPoints.at(0), subselectedPatterns, cameraMatrices[xxx], distCoeffVecs[xxx], errValues);
                
                ROS_INFO("Extended Reprojection error = %f", extendedReprojectionError_intrinsics[xxx]);        
        }
        

        
}

bool calibratorData::obtainStartingData(ros::NodeHandle& nh) {
        
        nh.param<std::string>("video_stream", video_stream, "video_stream");
        
        if (video_stream != "video_stream") {
                printf("%s << Video stream (%s) selected.\n", __FUNCTION__, video_stream.c_str());
        } else {
                printf("%s << ERROR! No video stream specified.\n", __FUNCTION__);
                return false;
        }
        
        nh.param<std::string>("video_sequence", video_sequence, "video_sequence");
        
        if (video_sequence != "video_sequence") {
                printf("%s << Video sequence (%s) selected.\n", __FUNCTION__, video_sequence.c_str());
        } else {
                printf("%s << ERROR! No video sequence specified.\n", __FUNCTION__);
                return false;
        }
        

        nh.param<bool>("useDefinedTopics", useDefinedTopics, false);

        nh.param<std::string>("video_streamLeft", video_streamLeft, "video_streamLeft");
        nh.param<std::string>("video_streamRight", video_streamRight, "video_streamRight");

        nh.param<std::string>("video_sequenceLeft", video_sequenceLeft, "video_sequenceLeft");
        nh.param<std::string>("video_sequenceRight", video_sequenceRight, "video_sequenceRight");


        if (useDefinedTopics){

            if (video_streamLeft != "video_streamLeft") {
                printf("%s << Video streamLeft (%s) selected.\n", __FUNCTION__, video_streamLeft.c_str());
            } else {
                printf("%s << ERROR! No video stream specified.\n", __FUNCTION__);
                return false;
            }

            if (video_sequenceLeft != "video_sequenceLeft") {
                printf("%s << Video sequenceLeft (%s) selected.\n", __FUNCTION__, video_sequenceLeft.c_str());
            } else {
                printf("%s << ERROR! No video sequence specified.\n", __FUNCTION__);
                return false;
            }


            if (video_streamRight != "video_streamRight") {
                printf("%s << Video streamRight (%s) selected.\n", __FUNCTION__, video_streamRight.c_str());
            } else {
                printf("%s << ERROR! No video stream specified.\n", __FUNCTION__);
                return false;
            }

            if (video_sequenceRight != "video_sequenceRight") {
                printf("%s << Video sequenceRight (%s) selected.\n", __FUNCTION__, video_sequenceRight.c_str());
            } else {
                printf("%s << ERROR! No video sequence specified.\n", __FUNCTION__);
                return false;
            }

        }
        //*/

        //HGH
        nh.param<double>("flowThreshold", flowThreshold, 1e-4);
        nh.param<double>("maxFrac", maxFrac, 0.05);
        nh.param<double>("errorThreshold", errorThreshold, 0.0);

        nh.param<double>("candidatePatternSampleRate", candidatePatternSampleRate, 1.0);
        nh.param<double>("testPatternSampleRate", testPatternSampleRate, 1.0);
        
        ROS_INFO("Candidates selected at (%f) and tested at (%f) probability.", candidatePatternSampleRate, testPatternSampleRate);
        
        nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
        
        if (outputFolder != "outputFolder") {
                ROS_WARN("Output folder (%s) selected.", outputFolder.c_str());
        } else {
                outputFolder = read_addr + "/../nodes/calibrator/data";
                ROS_WARN("No output folder supplied. Defaulting to (%s)", outputFolder.c_str());
        }
        
        nh.param<std::string>("calibType", calibType, "calibType");
        
        if (calibType == "single") {
                ROS_INFO("Single-camera calibration selected.");
        } else if (calibType == "stereo") {
                ROS_INFO("Stereo calibration selected.");
        } else if (calibType == "calibType") {
                ROS_WARN("No calibration type specified. Single-camera assumed.");
                calibType = "single";
        } else {
                ROS_ERROR("Invalid calibration type specified. Please choose either 'single' or 'stereo'.");
                return false;
        }
        
        nh.param<bool>("debug_mode", debugMode, false);
        
        if (debugMode) {
                printf("%s << Running in DEBUG mode.\n", __FUNCTION__);
        } else {
                printf("%s << Running in OPTIMIZED mode.\n", __FUNCTION__);
        }
        
        nh.param<std::string>("patternType", patternType, "mask");
        
        if (patternType == "mask") {
                pattType = MASK_FINDER_CODE;
        } else if (patternType == "chessboard") {
                pattType = CHESSBOARD_FINDER_CODE;
        } else if (patternType == "heated_chessboard") {
                pattType = HEATED_CHESSBOARD_FINDER_CODE;
        } else {
                
                printf("%s::%s << ERROR! patternType incorrectly specified...\n", __PROGRAM__, __FUNCTION__);
                return false;
                
        }
        
        printf("%s::%s << selected patternType = (%s) [%d]\n", __PROGRAM__, __FUNCTION__, patternType.c_str(), pattType);
        
        nh.param<int>("maxFrames", maxFrames, DEFAULT_MAX_FRAMES);
        nh.param<int>("maxPatterns", maxPatterns, DEFAULT_MAX_PATTERNS);
        nh.param<int>("setSize", setSize, DEFAULT_SET_SIZE);
        
        ROS_INFO("maxFrames = %d; maxPatterns = %d; setSize = (%d)", maxFrames, maxPatterns, setSize);
        
        nh.param<double>("gridSize", gridSize, DEFAULT_GRID_SIZE);
        
        nh.param<double>("maxTimeDiff", maxTimeDiff, DEFAULT_MAX_TIME_DIFF);
        
        nh.param<int>("xCount", xCount, DEFAULT_X_COUNT);
        nh.param<int>("yCount", yCount, DEFAULT_Y_COUNT);
        
        nh.param<std::string>("optMethod", optMethod, "enhancedMCM");
        
        if (optMethod == "enhancedMCM") {
                optimizationMethod = ENHANCED_MCM_OPTIMIZATION_CODE;
        } else if (optMethod == "allPatterns") {
                optimizationMethod = ALL_PATTERNS_OPTIMIZATION_CODE;
        } else if (optMethod == "randomSet") {
                optimizationMethod = RANDOM_SET_OPTIMIZATION_CODE;
        } else if (optMethod == "firstN") {
                optimizationMethod = FIRST_N_PATTERNS_OPTIMIZATION_CODE;
        } else if (optMethod == "randomBest") {
                optimizationMethod = BEST_OF_RANDOM_PATTERNS_OPTIMIZATION_CODE;
        } else if (optMethod == "exhaustiveSearch") {
                optimizationMethod = EXHAUSTIVE_SEARCH_OPTIMIZATION_CODE;
        } else if (optMethod == "randomSeed") {
                optimizationMethod = RANDOM_SEED_OPTIMIZATION_CODE;
        } else if (optMethod == "scoreBased") {
                optimizationMethod = SCORE_BASED_OPTIMIZATION_CODE;
        } else {
                
                printf("%s::%s << ERROR! optMethod incorrectly specified...\n", __PROGRAM__, __FUNCTION__);
                return false;
                
        }
        
        nh.param<bool>("undistortImages", undistortImages, false);
        
        nh.param<bool>("rectifyImages", rectifyImages, false);

        nh.param<bool>("wantsIntrinsics", wantsIntrinsics, false);

                nh.param<bool>("useIntrinsicsFile", useIntrinsicsFile, false);
                nh.param<std::string>("intrinsicsFileLeft", intrinsicsFileLeft, "intrinsicsFileLeft");
                nh.param<std::string>("intrinsicsFileRight", intrinsicsFileRight, "intrinsicsFileRight");
        //*/
        
        if (calibType == "single") {
                
                if (wantsIntrinsics == false) {
                        ROS_WARN("(single-cam mode): intrinsics will be calculated regardless of whether this option has been selected.");
                        wantsIntrinsics = true;
                }
                
        } else if (calibType == "stereo") {
                
                if (wantsIntrinsics) {
                        ROS_INFO("(stereo mode): option to also calculate intrinsics has been selected.");
                }
                else{

                        if (useIntrinsicsFile){

                                if (intrinsicsFileLeft =="intrinsicsFileLeft" || intrinsicsFileRight =="intrinsicsFileRight"){

                                        ROS_WARN("(use intrinsics): intrinsics files not specified correctly. Trying to use intrinsics from message header.");
                                        useIntrinsicsFile = false;
                                }
                        } else {
                                ROS_INFO("(use intrinsics): use intrinsics from message header.");
                        }
                }
        //*/
                
        }
        
        nh.param<double>("alpha", alpha, 0.00);
        
        nh.param<bool>("autoAlpha", autoAlpha, true);
        
        //HGH
        nh.param<bool>("stopCapturing", stopCapturing, false);
        nh.param<std::string>("patternDetectionMode", patternDetectionMode, "find");

        //HGH
        nh.param<double>("contrastLeft", contrastLeft, 1.0);
        nh.param<double>("contrastRight", contrastRight, 1.0);
        nh.param<double>("brightnessLeft", brightnessLeft, 0.0);
        nh.param<double>("brightnessRight", brightnessRight, 0.0);
        nh.param<bool>("normLeft", normLeft, false);
        nh.param<bool>("normRight", normRight, false);
        nh.param<bool>("invertLeft", invertLeft, false);
        nh.param<bool>("invertRight", invertRight, false);

        //HGH
        nh.param<bool>("doOnlineProcessing", doOnlineProcessing, true);
        nh.param<bool>("useMutex", useMutex, false);

        //HGH
         nh.param<bool>("adjustMSERLeft", adjustMSERLeft, false);
         nh.param<int>("deltaLeft", deltaLeft, 8);
         nh.param<double>("maxVarLeft", maxVarLeft, 0.25);
         nh.param<double>("minDivLeft", minDivLeft, 0.2);
         nh.param<double>("areaThresholdLeft", areaThresholdLeft, 1.01);

         nh.param<bool>("adjustMSERRight", adjustMSERRight, false);
         nh.param<int>("deltaRight", deltaRight, 8);
         nh.param<double>("maxVarRight", maxVarRight, 0.25);
         nh.param<double>("minDivRight", minDivRight, 0.2);
         nh.param<double>("areaThresholdRight", areaThresholdRight, 1.01);

        
        return true;
}

void calibratorNode::updateIntrinsicMap(unsigned int idx) {
        
        ROS_INFO("Updating map...");
        
        if (configData.autoAlpha) {
                configData.alpha = findBestAlpha(cameraMatrices[idx], distCoeffVecs[idx], imSize[idx]);
                ROS_INFO("Optimal alpha = (%f)", configData.alpha);
        }
        
        Rect* validPixROI = 0;
        bool centerPrincipalPoint = true;
        
        newCamMat[idx] = getOptimalNewCameraMatrix(cameraMatrices[idx], distCoeffVecs[idx], imSize[idx], configData.alpha, imSize[idx], validPixROI, centerPrincipalPoint);
                
        ROS_WARN("Adjusting undistortion mapping...");
        initUndistortRectifyMap(cameraMatrices[idx], distCoeffVecs[idx], default_R, newCamMat[idx], imSize[idx], CV_32FC1, map1[idx], map2[idx]);

        //HGH - debug
        cout << "newCamMat: " << newCamMat[idx] << endl;
        
        alphaChanged = false;
        
}
    
void calibratorNode::assignDebugCameraInfo() {
        
        // ..?
        

}

bool calibratorNode::isStillCollecting() {
                return stillCollecting;
}


bool calibratorNode::isVerifying() {
                return doVerify;
}


bool calibratorNode::findPattern(const Mat& im, vector<Point2f>& dst, Mat& prev, const int useMode = PATTERN_CODE_INVALID, const int cameraNumber = -1) {
        
    //HGH
    bool retVal = false;
    Mat tmpMat;


    if (useMode == PATTERN_CODE_FIND_TRACK){
    //###################################
    //find pattern first, then track:

    vector<Point2f> dstOrig = dst;
    dst.clear();

    //ROS_WARN("Finding Pattern");
    switch (configData.pattType) {
    case CHESSBOARD_FINDER_CODE:
        retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
        break;
    case MASK_FINDER_CODE:
        if (configData.adjustMSERLeft && cameraNumber == 0){
            retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaLeft, configData.maxVarLeft, configData.minDivLeft);
        }else if (configData.adjustMSERRight && cameraNumber == 1){
            retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaRight, configData.maxVarRight, configData.minDivRight);
        }else{
            retVal = findMaskCorners_1(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
        }
        break;
    case HEATED_CHESSBOARD_FINDER_CODE:
        invertMatIntensities(im, tmpMat);
        retVal = findChessboardCorners(tmpMat, cvSize(configData.xCount, configData.yCount), dst);
        break;
    default:
        retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
        break;
    }

    if (retVal){
        ROS_ERROR("Pattern found");
        return true;

    }else{

        dst = dstOrig;

        if ((configData.trackingMode) && (dst.size() > 0) && (prev.rows > 0) && (prev.cols > 0)) {
            // attempt to track, if it doesn't work, attempt re-detect..

            //HGH
            double distanceConstraint = (min(im.cols, im.rows) * configData.maxFrac) + ((((int) (max(im.cols, im.rows) * configData.maxFrac)) + 1) % 2);

            vector<Point2f> src;
            vector<unsigned int> lost;
            lost.clear();
            src.insert(src.end(), dst.begin(), dst.end());
            dst.clear();

            ROS_WARN("ATTEMPTING TO TRACK...");

            //ROS_INFO("im data = (%d, %d) : (%d, %d)", prev.rows, prev.cols, im.rows, im.cols);


            trackPoints2(prev, im, src, dst, distanceConstraint, configData.flowThreshold, lost, cvSize(configData.xCount, configData.yCount), configData.errorThreshold);

            // Then check if it's tracked effectively...

            if (lost.size() > 0) {
                ROS_ERROR("TRACKING FAILED");
                dst.clear();
                retVal = false;
            } else {
                ROS_WARN("TRACKING SUCCESSFUL");
                //HGH
                retVal = true;
            }

        } else {
            ROS_ERROR("Tracking not even possible");
            //ROS_ERROR("NOT EVEN TRYING TO TRACK: trackingMode=%d, dst.size()=%d", configData.trackingMode ? 1 : 0, dst.size());
            dst.clear();

        }

    } //*/


    }else if (useMode == PATTERN_CODE_TRACK_FIND){
    //###################################
    //track first then find pattern:


    if ((configData.trackingMode) && (dst.size() > 0) && (prev.rows > 0) && (prev.cols > 0)) {
        // attempt to track, if it doesn't work, attempt re-detect..

        //HGH
        double distanceConstraint = (min(im.cols, im.rows) * configData.maxFrac) + ((((int) (max(im.cols, im.rows) * configData.maxFrac)) + 1) % 2);

        vector<Point2f> src;
        vector<unsigned int> lost;
        lost.clear();
        src.insert(src.end(), dst.begin(), dst.end());
        dst.clear();

        ROS_WARN("ATTEMPTING TO TRACK...");

        //ROS_INFO("im data = (%d, %d) : (%d, %d)", prev.rows, prev.cols, im.rows, im.cols);


        trackPoints2(prev, im, src, dst, distanceConstraint, configData.flowThreshold, lost, cvSize(configData.xCount, configData.yCount), configData.errorThreshold);

        // Then check if it's tracked effectively...

        if (lost.size() > 0) {
            ROS_ERROR("TRACKING FAILED");
            dst.clear();
            retVal = false;
        } else {
            ROS_WARN("TRACKING SUCCESSFUL");
            //HGH
            retVal = true;
        }

    } else {
        ROS_ERROR("NOT EVEN TRYING TO TRACK: trackingMode=%d, dst.size()=%d", configData.trackingMode ? 1 : 0, dst.size());
        dst.clear();

    }

    if (dst.size() == 0) {

        //ROS_WARN("Finding Pattern");
        switch (configData.pattType) {
        case CHESSBOARD_FINDER_CODE:
            retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
            break;
        case MASK_FINDER_CODE:
            if (configData.adjustMSERLeft && cameraNumber == 0){
                retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaLeft, configData.maxVarLeft, configData.minDivLeft);
            }else if (configData.adjustMSERRight && cameraNumber == 1){
                retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaRight, configData.maxVarRight, configData.minDivRight);
            }else{
                retVal = findMaskCorners_1(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
            }
            break;
        case HEATED_CHESSBOARD_FINDER_CODE:
            invertMatIntensities(im, tmpMat);
            retVal = findChessboardCorners(tmpMat, cvSize(configData.xCount, configData.yCount), dst);
            break;
        default:
            retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
            break;
        }

    }
  //*/

    }else if (useMode == PATTERN_CODE_FIND){

    //###################################
    //only find pattern, no tracking:

   // /*
        //ROS_WARN("Finding Pattern");
        switch (configData.pattType) {
        case CHESSBOARD_FINDER_CODE:
            retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
            break;
        case MASK_FINDER_CODE:
            if (configData.adjustMSERLeft && cameraNumber == 0){
                retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaLeft, configData.maxVarLeft, configData.minDivLeft);
            }else if (configData.adjustMSERRight && cameraNumber == 1){
                retVal = findMaskCorners_2(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.deltaRight, configData.maxVarRight, configData.minDivRight);
            }else{
                retVal = findMaskCorners_1(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
            }
            break;
        case HEATED_CHESSBOARD_FINDER_CODE:
            invertMatIntensities(im, tmpMat);
            retVal = findChessboardCorners(tmpMat, cvSize(configData.xCount, configData.yCount), dst);
            break;
        default:
            retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
            break;
        }

        if (retVal){
            ROS_INFO("PATTERNFINDING SUCCESFUL");
        }else{
            ROS_WARN("PATTERNFINDING FAILED");
        }


      } else{
        ROS_ERROR("PATTERN FIND MODE IS INVALID!");
    }


    return retVal;
                
}



void calibratorNode::trackPattern(int cameraNumber, const Mat& currentImg, const int currentFrame){

    bool patternFound = false;

    Mat grayImg;

    if (currentImg.type() == CV_8UC3){
        cvtColor(currentImg, grayImg, CV_RGB2GRAY);
    }else{
        grayImg = Mat(currentImg);
    }

    patternFound = findPattern(grayImg, cornerSets[cameraNumber], lastMat[cameraNumber],configData.usePatternDetectionMode, cameraNumber);

    if (configData.debugMode) {

        Mat dispMat;

        if (currentImg.type() == CV_8UC3) {
            currentImg.copyTo(dispMat);
        } else {
            cvtColor(currentImg, dispMat, CV_GRAY2RGB);
        }


        drawChessboardCorners(dispMat, cvSize(configData.xCount, configData.yCount), Mat(cornerSets[cameraNumber]), patternFound);


        if (cameraNumber == 0){

            if (msg_debug_1.width > 0) {
                std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_1.data.begin());
            }

            debug_pub_1.publish(msg_debug_1, debug_camera_info_1);

        } else if (cameraNumber == 1){

            if (msg_debug_2.width > 0) {
                std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_2.data.begin());
            }

            debug_pub_2.publish(msg_debug_2, debug_camera_info_2);

        }
    }

      if (patternFound) {
            pointSets[cameraNumber].push_back(cornerSets[cameraNumber]);
            frameCounts[cameraNumber].push_back(currentFrame); // contains all frames where pattern was found
            grayImg.copyTo(lastMat[cameraNumber]);
            ROS_INFO("For cam (%d), (%d) patterns found.",cameraNumber, pointSets[cameraNumber].size());
//        }else{
//          cornerSets[cameraNumber].clear();
//          lastMat[cameraNumber] = Mat();
      }
}


void calibratorNode::determineValidPairs() {

        ROS_INFO("Determine valid frame pairs...");

        int maxCam1 = std::min(((int) times_1.size()), frameCount_1);
        int maxCam2 = std::min(((int) times_2.size()), frameCount_2);


        //Mat dispMat1, dispMat2;
        //ROS_WARN("HERE BEGIN---");
        while (checkIndex_1 < maxCam1) {
                
                if (checkIndex_1 >= duplicateFlags_1.size()) {
                        break;
                }
                
                //ROS_WARN("checkIndex_1 %d-",checkIndex_1);

                if  (duplicateFlags_1.at(checkIndex_1) == 1) {
                        checkIndex_1++;
                        continue;
                }
                
                //ROS_WARN("- checkIndex_1 %d-",checkIndex_1);
                //ROS_WARN("times_1.size() %d-",times_1.size());
                //cout << times_1.at(checkIndex_1) << endl;


                double leastDiff = 9e99;
                int bestMatch = -1;
                
                bool candidatesExhausted = false;
        
                //ROS_WARN("checkIndex_2 %d-",checkIndex_2);
                //ROS_WARN("times_2.size() %d-",times_2.size());
                //cout << times_2.at(checkIndex_2) << endl;

                //cout << "m(timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) < configData.maxTimeDiff) " << (timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) < configData.maxTimeDiff) << endl;
                //ROS_WARN("diffdone");
                
                while ((timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) < configData.maxTimeDiff)) {
                        
                        //ROS_WARN("maxCam2-1= %d-",maxCam2-1);
                        //ROS_WARN("checkIndex_2 %d-",checkIndex_2);
                        if (checkIndex_2 >= (maxCam2-1)) {
                                ROS_WARN("...");
                                break;
                                
                        }

                        if (checkIndex_2 >= duplicateFlags_2.size()) {
                            break;
                        }

                        //ROS_WARN("DEUBG %s", 3);
                        if  (duplicateFlags_2.at(checkIndex_2) == 1) {
                                checkIndex_2++;
                                continue;
                        }
                        //ROS_WARN("DEUBG %s", 4);
                        double diff = abs(timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)));
                        
                        
                       // ROS_WARN("DEUBG %s", 5);
                        if (diff < leastDiff) {
                                leastDiff = diff;
                                bestMatch = checkIndex_2;
                        }
                        
                        checkIndex_2++;
                }

                //ROS_WARN("DEUBG %s", 10);
                
                if ((timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) >= configData.maxTimeDiff)) {
                        candidatesExhausted = true;
                }
                
                //checkIndex_2 = bestMatch;
                
                if ((leastDiff < configData.maxTimeDiff) && (bestMatch >= 0)) {
                        
                        //ROS_WARN("Pushing back match (%f)  (%d) [%02d.%04d] and (%d) [%02d.%04d]...", leastDiff, checkIndex_1, times_1.at(checkIndex_1).sec, times_1.at(checkIndex_1).nsec, bestMatch, times_2.at(bestMatch).sec, times_2.at(bestMatch).nsec);
                        
                        validPairs[0].push_back(checkIndex_1);
                        checkIndex_1++;
                        validPairs[1].push_back(bestMatch);


                } else if (candidatesExhausted) { // (checkIndex_2 < (maxCam2-1)) {
                        checkIndex_1++;
                } else {
                        break;
                }
                
        }
                        
}


void calibratorNode::evaluateFrames() {

     ROS_WARN("evaluateFrames...");

     if (!configData.doOnlineProcessing){

        ROS_INFO("Track pattern offline...");

        pointSets[0].clear();
        pointSets[1].clear();
        frameCounts[0].clear();
        frameCounts[1].clear();

        Mat dispMat;

        //cam1
        for (int i=0; i < displayImages_1.size() ; i++){

            dispMat = displayImages_1.at(i);

            trackPattern(0, dispMat, i); //cam1
        }
        //cam2
        for (int i=0; i < displayImages_2.size() ; i++){

            dispMat = displayImages_2.at(i);

            trackPattern(1, dispMat, i); //cam2
        }

     }

        vector<validPattern> validPatternData[2];


        // check if pattern was found in valid frame:

        for (int camNumber = 0; camNumber <= 2; camNumber++ ){

            for (int i = 0; i < validPairs[camNumber].size(); i++){

                for (int j = 0; j < frameCounts[camNumber].size(); j++){

                    validPattern tmpData;
                    tmpData.validFrameID = validPairs[camNumber].at(i);
                    tmpData.patternID = j;


                    if(validPairs[camNumber].at(i)==frameCounts[camNumber].at(j)){

                        tmpData.patternFound = true;
                        tmpData.isValidFrame = true;

                        validPatternData[camNumber].push_back(tmpData);
                        break;


                        ROS_INFO("Valid pattern found for cam (%d) in frame %d", camNumber, frameCounts[camNumber].at(j));

                    } else if(j==frameCounts[camNumber].size()-1){

                        //no valid pattern found for valid pair ///stimmt loop?

                        tmpData.patternFound = false;
                        tmpData.isValidFrame = true;

                         validPatternData[camNumber].push_back(tmpData);

                    }
                }
            }
        }


        Mat dispMat1, dispMat2;



        int totalValidPatterns = 0;

        for(int i=0; i < validPatternData[0].size(); i++){

            if ( validPatternData[0].at(i).patternFound && validPatternData[1].at(i).patternFound){

                 if ( !configData.debugMode) {
                    extrinsicsPointSets[0].push_back(pointSets[0].at(validPatternData[0].at(i).patternID));
                    extrinsicsPointSets[1].push_back(pointSets[1].at(validPatternData[1].at(i).patternID));
                 }
                totalValidPatterns++;

            }

        }

        cout << "synced frames: " << validPairs[0].size() << endl;
        cout << "cam1 patterns found: " <<  frameCounts[0].size() << endl;
        cout << "cam2 patterns found: " <<  frameCounts[1].size() << endl;

        cout << "valid pairs: " <<  validPatternData[0].size() << endl;
        cout << "total valid patterns: " << totalValidPatterns << endl;



        if ( configData.debugMode) {

            doVerify = true;

            int currentValidPattern = 0;

            for(int i=0; i < validPatternData[0].size(); i++){

                if ( validPatternData[0].at(i).patternFound && validPatternData[1].at(i).patternFound){

                    ROS_INFO("\nValid pattern pair %d out of  %d :  frame cam1: %d, frame cam2 %d ", currentValidPattern + 1, totalValidPatterns, validPatternData[0].at(i).patternID ,  validPatternData[1].at(i).patternID );



                    int dispFrame1 = validPatternData[0].at(i).validFrameID;
                    int dispFrame2 = validPatternData[1].at(i).validFrameID;

                    int dispPattern1 = validPatternData[0].at(i).patternID;
                    int dispPattern2 = validPatternData[1].at(i).patternID;

                    if (displayImages_1.at(dispFrame1).type() == CV_8UC3) {
                        displayImages_1.at(dispFrame1).copyTo(dispMat1);
                    } else {
                        cvtColor(displayImages_1.at(dispFrame1), dispMat1, CV_GRAY2RGB);
                    }


                    drawChessboardCorners(dispMat1, cvSize(configData.xCount, configData.yCount), Mat(pointSets[0].at(dispPattern1)), true);


                    if (displayImages_2.at(dispFrame2).type() == CV_8UC3) {
                        displayImages_2.at(dispFrame2).copyTo(dispMat2);
                    } else {
                        cvtColor(displayImages_2.at(dispFrame2), dispMat2, CV_GRAY2RGB);
                    }

                    drawChessboardCorners(dispMat2, cvSize(configData.xCount, configData.yCount), Mat(pointSets[1].at(dispPattern2)), true);


                    if(currentValidPattern < totalValidPatterns - 1){

                        imshow("pattern1", dispMat1);
                        imshow("pattern2", dispMat2);

                        ROS_INFO("%d << Press 'c' to use this keyframes or any other key to discard...", i);


                        char key = waitKey();
                        if (key == 'c'){
                            extrinsicsPointSets[0].push_back(pointSets[0].at(validPatternData[0].at(i).patternID));
                            extrinsicsPointSets[1].push_back(pointSets[1].at(validPatternData[1].at(i).patternID));
                            ROS_INFO("Added patterns for valid pair %d...", currentValidPattern);
                        }

                        currentValidPattern++;

                    } else {
                        destroyWindow("pattern1");
                        destroyWindow("pattern2");
                        doVerify = false;
                    }

                }

            }

        }

}

//preprocess images
void calibratorNode::preprocessImage(Mat src, Mat &dst, double a = 1.0, double b = 0.0, bool normalize = false, bool negative = false){
\

    Mat newImage = Mat::zeros(src.size(), src.type());

    if (src.type() == CV_8UC1){

        for (int j = 0; j < src.rows; j++){
            for (int i = 0; i < src.cols; i++){
                    newImage.at<uchar>(j,i) = saturate_cast<uchar> (a * (src.at<uchar>(j,i))+ b);
            }
        }

    }else{

        for (int j = 0; j < src.rows; j++){
            for (int i = 0; i < src.cols; i++){
                for (int c = 0; c< 3; c++){
                    newImage.at<Vec3b>(j,i)[c] = saturate_cast<uchar> (a * (src.at<Vec3b>(j,i)[c])+ b);
                }
            }
        }

    }


    //normalize
    if (normalize){
        cvtColor(newImage, src, CV_RGB2GRAY);
        equalizeHist(src,newImage);
    }


    //invert

    if (negative){

    if (src.type() == CV_8UC1){

        for (int j = 0; j < newImage.rows; j++){
            for (int i = 0; i < newImage.cols; i++){
                    newImage.at<uchar>(j,i) = 255 - saturate_cast<uchar> (newImage.at<uchar>(j,i));
            }
        }

    }else{

        for (int j = 0; j < newImage.rows; j++){
            for (int i = 0; i < newImage.cols; i++){
                for (int c = 0; c< 3; c++){
                    newImage.at<Vec3b>(j,i)[c] = 255 - saturate_cast<uchar> ( newImage.at<Vec3b>(j,i)[c]);
                }
            }
        }

    }

    }




    dst = Mat(newImage);

}


void calibratorNode::updatePairs() {


    //HGH
    determineValidPairs();
    Mat dispMat1, dispMat2;
    /* HGH

        int maxCam1 = std::min(((int) times_1.size()), frameCount_1);
        int maxCam2 = std::min(((int) times_2.size()), frameCount_2);

        Mat dispMat1, dispMat2;

        while (checkIndex_1 < maxCam1) {

                if (checkIndex_1 >= duplicateFlags_1.size()) {
                        break;
                }

                if  (duplicateFlags_1.at(checkIndex_1) == 1) {
                        checkIndex_1++;
                        continue;
                }

                double leastDiff = 9e99;
                int bestMatch = -1;

                bool candidatesExhausted = false;


                while ((timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) < configData.maxTimeDiff)) {

                        if (checkIndex_2 >= (maxCam2-1)) {
                                break;

                        }

                        if (checkIndex_2 >= duplicateFlags_2.size()) {
                                break;
                        }

                        if  (duplicateFlags_2.at(checkIndex_2) == 1) {
                                checkIndex_2++;
                                continue;
                        }

                        double diff = abs(timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)));



                        if (diff < leastDiff) {
                                leastDiff = diff;
                                bestMatch = checkIndex_2;
                        }

                        checkIndex_2++;
                }

                if ((timeDiff(times_2.at(checkIndex_2), times_1.at(checkIndex_1)) >= configData.maxTimeDiff)) {
                        candidatesExhausted = true;
                }

                //checkIndex_2 = bestMatch;

                if ((leastDiff < configData.maxTimeDiff) && (bestMatch >= 0)) {

                        //ROS_WARN("Pushing back match (%f)  (%d) [%02d.%04d] and (%d) [%02d.%04d]...", leastDiff, checkIndex_1, times_1.at(checkIndex_1).sec, times_1.at(checkIndex_1).nsec, bestMatch, times_2.at(bestMatch).sec, times_2.at(bestMatch).nsec);

                        validPairs[0].push_back(checkIndex_1);
                        checkIndex_1++;
                        validPairs[1].push_back(bestMatch);
                } else if (candidatesExhausted) { // (checkIndex_2 < (maxCam2-1)) {
                        checkIndex_1++;
                } else {
                        break;
                }

        }

        //*/

        if (validPairs[0].size() > publishCount) {

                int currentPair = validPairs[0].size()-1;

                if (currentPair == -1) {
                        return;
                }



                // This is probably where it's worth attempting to find the patterns

                bool patternFound_1 = false, patternFound_2 = false;

                Mat tmpMat1, tmpMat2;

                vector<Point2f> cornerSet_1, cornerSet_2;


                if ((validPairs[0].at(currentPair) >= displayImages_1.size()) || (validPairs[1].at(currentPair) >= displayImages_2.size())) {
                        ROS_WARN("Skipping detection because there appear to be insufficient buffered frames [(%d)(%d) : (%d)(%d)", validPairs[0].at(currentPair), displayImages_1.size(), validPairs[1].at(currentPair), displayImages_2.size());
                        return;
                }

                //ROS_WARN("Processing pair (%d) (%d, %d)[sync'ed: %f]", currentPair, validPairs[0].at(currentPair), validPairs[1].at(currentPair), timeDiff(times_2.at(validPairs[1].at(currentPair)), times_1.at(validPairs[0].at(currentPair))));

                Mat grayMat1, grayMat2;

                if (displayImages_1.at(validPairs[0].at(currentPair)).type() == CV_8UC3) {
                        cvtColor(displayImages_1.at(validPairs[0].at(currentPair)), grayMat1, CV_RGB2GRAY);
                } else {
                        grayMat1 = Mat(displayImages_1.at(validPairs[0].at(currentPair)));
                }

                //ROS_WARN("First image retrieved..");

                if (displayImages_2.at(validPairs[1].at(currentPair)).type() == CV_8UC3) {
                        cvtColor(displayImages_2.at(validPairs[1].at(currentPair)), grayMat2, CV_RGB2GRAY);
                } else {
                        grayMat2 = Mat(displayImages_2.at(validPairs[1].at(currentPair)));
                }


                switch (configData.pattType) {
                        case CHESSBOARD_FINDER_CODE:
                                patternFound_1 = findChessboardCorners(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);
                                patternFound_2 = findChessboardCorners(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);
                                break;
                        case MASK_FINDER_CODE:
                                patternFound_1 = findMaskCorners_1(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
                                patternFound_2 = findMaskCorners_1(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
                                break;
                        case HEATED_CHESSBOARD_FINDER_CODE:
                                invertMatIntensities(grayMat1, tmpMat1);
                                patternFound_1 = findChessboardCorners(tmpMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);

                                invertMatIntensities(grayMat2, tmpMat2);
                                patternFound_2 = findChessboardCorners(tmpMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);

                                break;
                        default:
                                patternFound_1 = findChessboardCorners(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);
                                patternFound_2 = findChessboardCorners(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);
                                break;
                }

                if (patternFound_1 && patternFound_2) {

                        extrinsicsPointSets[0].push_back(cornerSet_1);
                        extrinsicsPointSets[1].push_back(cornerSet_2);
                        ROS_INFO("(%d) patterns found from (%d) valid frame pairs...", extrinsicsPointSets[0].size(), validPairs[0].size());
                        pointSets[0].push_back(cornerSet_1);
                        pointSets[1].push_back(cornerSet_2);
                        //ROS_INFO("For cam (0), (%d) patterns found.", pointSets[0].size());
                        //ROS_INFO("For cam (1), (%d) patterns found.", pointSets[1].size());
                } else if (patternFound_1) {
                        pointSets[0].push_back(cornerSet_1);
                        //ROS_INFO("For cam (0), (%d) patterns found.", pointSets[0].size());
                } else if (patternFound_2) {
                        pointSets[1].push_back(cornerSet_2);
                        //ROS_INFO("For cam (1), (%d) patterns found.", pointSets[1].size());
                }

                if (configData.debugMode) {

                        if (displayImages_1.at(validPairs[0].at(currentPair)).type() == CV_8UC3) {
                                displayImages_1.at(validPairs[0].at(currentPair)).copyTo(dispMat1);
                        } else {
                                cvtColor(displayImages_1.at(validPairs[0].at(currentPair)), dispMat1, CV_GRAY2RGB);
                        }


                        drawChessboardCorners(dispMat1, cvSize(configData.xCount, configData.yCount), Mat(cornerSet_1), patternFound_1);

                        if (msg_debug_1.width > 0) {
                                std::copy(&(dispMat1.at<Vec3b>(0,0)[0]), &(dispMat1.at<Vec3b>(0,0)[0])+(dispMat1.cols*dispMat1.rows*dispMat1.channels()), msg_debug_1.data.begin());
                        }

                        if (displayImages_2.at(validPairs[1].at(currentPair)).type() == CV_8UC3) {
                                displayImages_2.at(validPairs[1].at(currentPair)).copyTo(dispMat2);
                        } else {
                                cvtColor(displayImages_2.at(validPairs[1].at(currentPair)), dispMat2, CV_GRAY2RGB);
                        }

                        drawChessboardCorners(dispMat2, cvSize(configData.xCount, configData.yCount), Mat(cornerSet_2), patternFound_2);

                        if (msg_debug_2.width > 0) {
                                std::copy(&(dispMat2.at<Vec3b>(0,0)[0]), &(dispMat2.at<Vec3b>(0,0)[0])+(dispMat2.cols*dispMat2.rows*dispMat2.channels()), msg_debug_2.data.begin());
                        }

                        //ROS_ERROR("Publishing frame pair...");
                        debug_pub_1.publish(msg_debug_1, debug_camera_info_1);
                        debug_pub_2.publish(msg_debug_2, debug_camera_info_2);

                }

                publishCount = validPairs[0].size();
                //publishCount++;
        }


}



void calibratorNode::handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
        //ROS_WARN("Handling image 1...");

        //HGH

    if(configData.stopCapturing){
        stillCollectingSet[0]=false;
        stillCollectingSet[1]=false;
        stillCollecting = false;
        return;
    }


        if ((frameCount_1 > configData.maxFrames)) {
        //if ((pointSets[0].size() >= configData.maxPatterns) || (frameCount_1 >= configData.maxFrames)) {

                stillCollectingSet[0]  = false;

                //HGH
                if (!stillCollectingSet[1]){
                    //ROS_ERROR("Max frames/patterns received...");
                    stillCollectingSet[1] = false;
                    stillCollecting = false;
                    return;
                }

                return;
        }

        while (!infoProcessed_1) {
                
                if (configData.wantsIntrinsics) {
                        infoProcessed_1 = true;
                } else {

                    //use provided intrinsics file
                    if (configData.useIntrinsicsFile){

                            ROS_WARN("(use intrinsics): intrinsicsFileLeft: %s", configData.intrinsicsFileLeft.c_str());

                            if (configData.intrinsicsFileLeft[0] != '/') {
                                    configData.intrinsicsFileLeft = configData.read_addr + configData.intrinsicsFileLeft;
                            }

                            // http://www.mathpirate.net/log/2009/11/26/when-in-doubt-link-debug/
                            Mat dummyImage(300, 300, CV_8UC1);
                            GaussianBlur(dummyImage, dummyImage, Size(5, 5), 1.5);

                            try {

                                    FileStorage fs(configData.intrinsicsFileLeft, FileStorage::READ);
                                    fs["cameraMatrix"] >> configData.K1;
                                    fs["distCoeffs"] >> configData.distCoeffs1;
                                    fs.release();

                                    //ros::Duration(0.5).sleep();

                                    ROS_INFO("Calibration data read.");

                                    if (configData.K1.empty()){
                                        ROS_ERROR("Intrinsics file %s invalid! Please check path and filecontent...\n", configData.intrinsicsFileLeft.c_str());
                                    }

                                    infoProcessed_1 = true;

                            }catch(int e){
                                    ROS_ERROR("Some failure in reading in the camera parameters...");
                            }

                    } else {
                    //extract intrinsics from msg header
                    //*///HGH end

                        
                        try     {
                                
                                configData.K1 = Mat::eye(3, 3, CV_64FC1);
                                
                                for (unsigned int mmm = 0; mmm < 3; mmm++) {
                                        for (unsigned int nnn = 0; nnn < 3; nnn++) {
                                                configData.K1.at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
                                        }
                                }
                                
                                unsigned int maxDistortionIndex;
                                if (info_msg->distortion_model == "plumb_bob") {
                                        maxDistortionIndex = 5;
                                } else if (info_msg->distortion_model == "rational_polynomial") {
                                        maxDistortionIndex = 8;
                                }
                                
                                configData.distCoeffs1 = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
                                
                                for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
                                        configData.distCoeffs1.at<double>(0, iii) = info_msg->D[iii];
                                }
                                
                                ROS_INFO("Camera 1 intrinsics:");
                                cout << configData.K1 << endl;
                                cout << configData.distCoeffs1 << endl;
                                infoProcessed_1 = true;
                        } catch (sensor_msgs::CvBridgeException& e) {
                                ROS_ERROR("Some failure in reading in the camera parameters...");
                        }

                        }//HGH
                } 
                
        }
        
        if (infoProcessed_1) {

                elapsedTime = timeElapsedMS(cycle_timer);
                
                if (frameCount_1 > 0) {
                        avgTime += elapsedTime;
                }
                
                times_1.push_back(info_msg->header.stamp);
                
                cv_ptr_1 = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);
                
                Mat newImage(cv_ptr_1->image);
                Mat currImage, gray16, dispMat;
                
                if (newImage.type() == CV_16UC1) {
                        //ROS_WARN("type = CV_16UC1");
                        adaptiveDownsample(newImage, currImage, 0);
                        newImage = Mat(currImage);
                } else if (newImage.type() == CV_16UC3) {
                        //ROS_WARN("type = CV_16UC3");
                        cvtColor(newImage, gray16, CV_RGB2GRAY);
                        adaptiveDownsample(gray16, currImage, 0);
                        newImage = Mat(currImage);
                }


                if (newImage.type() == CV_8UC1) {
                    cvtColor(newImage, dispMat, CV_GRAY2RGB);

                } else {
                        dispMat = Mat(newImage);
                }

                //HGH
                preprocessImage(dispMat, dispMat, configData.contrastLeft, configData.brightnessLeft, configData.normLeft, configData.invertLeft);

                
                // Debug Publication
                
                if (displayImages_1.size() > 0) {
                        if (matricesAreEqual(displayImages_1.at(displayImages_1.size()-1), dispMat)) {
                                duplicateFlags_1.push_back(1);
                                // ROS_WARN("Received duplicate frame at cam 1 (%d)", frameCount_1);
                        } else {
                                duplicateFlags_1.push_back(0);
                        }
                } else {
                        duplicateFlags_1.push_back(0);
                        imSize[0] = newImage.size();
                }
                
                displayImages_1.push_back(dispMat);
                
                if (configData.debugMode) {
                        
                        if (msg_debug_1.width == 0) {
                                msg_debug_1.width = dispMat.cols; 
                                msg_debug_1.height = dispMat.rows;
                                msg_debug_1.encoding = "bgr8";
                                msg_debug_1.is_bigendian = false;
                                msg_debug_1.step = dispMat.cols*3;
                                msg_debug_1.data.resize(dispMat.cols*dispMat.rows*dispMat.channels());
                        }
                }
                
                // Check for sync'ed frames here (only with camera 1)
                
                //ROS_WARN("DEBUG checking frames img1..");
                //HGH
                if(configData.useMutex){
                    if (_access.try_lock()) {
                        updatePairs();
                        _access.unlock();
                    }
                }else{

                    //HGH
                    if (configData.doOnlineProcessing){
                        trackPattern(0, dispMat, frameCount_1); //cam1
                    }else{
                        ROS_INFO("Cam 1: current frame %d", frameCount_1);
                        if (frameCount_1%20==0){
                            trackPattern(0, dispMat, frameCount_1);
                        }
                    }
                }
                
                frameCount_1++;
                
        }
}



void calibratorNode::handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
        //ROS_WARN("Handling image 2...");

    if (!stillCollecting){
        return;
    }
        

    //HGH
    if(configData.stopCapturing){
        stillCollectingSet[0]=false;
        stillCollectingSet[1]=false;
        stillCollecting = false;
        return;
    }

     if ((frameCount_2 >= configData.maxFrames)) {
   //if ((pointSets[1].size() >= configData.maxPatterns) || (frameCount_2 >= configData.maxFrames)) {

                stillCollectingSet[1] = false;

                //HGH
                if ( !stillCollectingSet[0]){
                    //ROS_ERROR("Max frames/patterns received...");
                    stillCollectingSet[0] = false;
                    stillCollecting = false;
                    return;
                }

                return;
        }


        
        while (!infoProcessed_2) {
                
                if (configData.wantsIntrinsics) {
                        infoProcessed_2 = true;
                } else {

                    //use provided intrinsics file
                    if (configData.useIntrinsicsFile){

                            ROS_WARN("(use intrinsics): intrinsicsFileRight: %s", configData.intrinsicsFileRight.c_str());

                            if (configData.intrinsicsFileRight[0] != '/') {
                                configData.intrinsicsFileRight = configData.read_addr + configData.intrinsicsFileRight;
                            }


                            // http://www.mathpirate.net/log/2009/11/26/when-in-doubt-link-debug/
                            Mat dummyImage(300, 300, CV_8UC1);
                            GaussianBlur(dummyImage, dummyImage, Size(5, 5), 1.5);

                            try {

                                    FileStorage fs(configData.intrinsicsFileRight, FileStorage::READ);
                                    fs["cameraMatrix"] >> configData.K2;
                                    fs["distCoeffs"] >> configData.distCoeffs2;
                                    fs.release();

                                    //ros::Duration(0.5).sleep();

                                    ROS_INFO("Calibration data read.");

                                    if (configData.K2.empty()){
                                        ROS_ERROR("Intrinsics file %s invalid! Please check path and filecontent...\n", configData.intrinsicsFileRight.c_str());
                                    }

                                    infoProcessed_2 = true;

                            }catch(int e){
                                    ROS_ERROR("Some failure in reading in the camera parameters...");
                            }

                    } else {
                    //extract intrinsics from msg header
                    //*///HGH end


                        try     {
                                
                                configData.K2 = Mat::eye(3, 3, CV_64FC1);
                                
                                for (unsigned int mmm = 0; mmm < 3; mmm++) {
                                        for (unsigned int nnn = 0; nnn < 3; nnn++) {
                                                configData.K2.at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
                                        }
                                }
                                
                                unsigned int maxDistortionIndex;
                                if (info_msg->distortion_model == "plumb_bob") {
                                        maxDistortionIndex = 5;
                                } else if (info_msg->distortion_model == "rational_polynomial") {
                                        maxDistortionIndex = 8;
                                }
                                
                                configData.distCoeffs2 = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
                                
                                for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
                                        configData.distCoeffs2.at<double>(0, iii) = info_msg->D[iii];
                                }
                                
                                ROS_INFO("Camera 2 intrinsics:");
                                cout << configData.K2 << endl;
                                cout << configData.distCoeffs2 << endl;
                                infoProcessed_2 = true;
                                
                                
                        } catch (sensor_msgs::CvBridgeException& e) {
                                ROS_ERROR("Some failure in reading in the camera parameters...");
                        }

                        }//HGH

                } 
                
        }
        
        if (infoProcessed_2) {

                times_2.push_back(info_msg->header.stamp);
                
                cv_ptr_2 = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);
                
                Mat newImage(cv_ptr_2->image);
                Mat currImage, gray16, dispMat;
                
                if (newImage.type() == CV_16UC1) {
                        //ROS_WARN("type = CV_16UC1");
                        adaptiveDownsample(newImage, currImage, 0);
                        newImage = Mat(currImage);
                } else if (newImage.type() == CV_16UC3) {
                        //ROS_WARN("type = CV_16UC3");
                        cvtColor(newImage, gray16, CV_RGB2GRAY);
                        adaptiveDownsample(gray16, currImage, 0);
                        newImage = Mat(currImage);
                }
                
                if (newImage.type() == CV_8UC1) {
                        cvtColor(newImage, dispMat, CV_GRAY2RGB); //orig
                } else {
                        dispMat = Mat(newImage); //orig
                }

                //HGH
                preprocessImage(dispMat, dispMat, configData.contrastRight, configData.brightnessRight, configData.normRight, configData.invertRight);
                
                
                // Debug Publication
                
                if (displayImages_2.size() > 0) {
                        if (matricesAreEqual(displayImages_2.at(displayImages_2.size()-1), dispMat)) {
                                duplicateFlags_2.push_back(1);
                                //ROS_WARN("Received duplicate frame at cam 2 (%d)", frameCount_2);
                        } else {
                                duplicateFlags_2.push_back(0);
                        }
                } else {
                        duplicateFlags_2.push_back(0);
                        imSize[1] = newImage.size();
                }
                
                displayImages_2.push_back(dispMat);

                
                if (configData.debugMode) {
                                
                        if (msg_debug_2.width == 0) {
                                msg_debug_2.width = dispMat.cols; 
                                msg_debug_2.height = dispMat.rows;
                                msg_debug_2.encoding = "bgr8";
                                msg_debug_2.is_bigendian = false;
                                msg_debug_2.step = dispMat.cols*3;
                                msg_debug_2.data.resize(dispMat.cols*dispMat.rows*dispMat.channels());
                        }
                }
                
                //ROS_WARN("DEBUG checking frames img2..");\
                //HGH
                if(configData.useMutex){
                    if (_access.try_lock()) {
                        updatePairs();
                        _access.unlock();
                    }
                }else{
                    //HGH
                    if (configData.doOnlineProcessing){
                        trackPattern(1,dispMat, frameCount_2); //cam2
                    }else{
                        ROS_INFO("Cam 2: current frame %d", frameCount_2);
                        if (frameCount_2%20==0){
                            trackPattern(1,dispMat, frameCount_2);
                        }
                    }
                }

                frameCount_2++;
                
        }
}

void calibratorNode::handle_image(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
        
        if ((pointSets[0].size() >= configData.maxPatterns) || (frameCount >= configData.maxFrames)) {
                ROS_INFO("Max frames/patterns received.");
                stillCollecting = false;
                return;
        }
        
        // Currently not doing any camera info processing...
        infoProcessed = true;
        
        if (infoProcessed) {
                
                elapsedTime = timeElapsedMS(cycle_timer);
                
                if (frameCount > 0) {
                        avgTime += elapsedTime;
                }
                
                cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                       // For some reason it reads as BGR, not gray

                Mat newImage(cv_ptr->image);
                
                if (imSize[0].width == 0) {
                        if (newImage.rows != 0) {
                                imSize[0] = newImage.size();
                        }
                }
                
                
                
                if (configData.debugMode) {
                        if (newImage.rows > 0) {
                                
                                Mat tmpCpy, grayMat, tmpMat, dispMat;
                                
                                if (newImage.type() == CV_8UC3) {
                                        tmpCpy = Mat(newImage);
                                        cvtColor(newImage, grayMat, CV_RGB2GRAY);
                                } else {
                                        grayMat = Mat(newImage);
                                        cvtColor(newImage, tmpCpy, CV_GRAY2RGB);
                                        
                                }
                                
                                displayImages_1.push_back(tmpCpy);
                                
                                
                                if (frameCount == 0) {
                                        msg_debug_1.width = tmpCpy.cols; 
                                        msg_debug_1.height = tmpCpy.rows;
                                        msg_debug_1.encoding = "bgr8";
                                        msg_debug_1.is_bigendian = false;
                                        msg_debug_1.step = tmpCpy.cols*3;
                                        msg_debug_1.data.resize(tmpCpy.cols*tmpCpy.rows*tmpCpy.channels());
                                }
                                
                                bool patternFound;
                                
                                //HGH
                                //vector<Point2f> cornerSet;
                                
                                //ROS_WARN("About to attempt pattern-finding...");
                                
                                //HGH
                                patternFound = findPattern(tmpCpy, cornerSet, prevMat, configData.usePatternDetectionMode);

//HGH
//                              switch (configData.pattType) {
//                                      case CHESSBOARD_FINDER_CODE:
//                                              patternFound = findChessboardCorners(tmpCpy, cvSize(configData.xCount, configData.yCount), cornerSet);
//                                              break;
//                                      case MASK_FINDER_CODE:
//                                              //printf("%s << here (%d, %d)\n", __FUNCTION__, configData.xCount, configData.yCount);
//                                              patternFound = findMaskCorners_1(tmpCpy, cvSize(configData.xCount, configData.yCount), cornerSet, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
//                                              break;
//                                      case HEATED_CHESSBOARD_FINDER_CODE:
//                                              invertMatIntensities(grayMat, tmpMat);
//                                              tmpMat.copyTo(grayMat);
//                                              patternFound = findChessboardCorners(tmpCpy, cvSize(configData.xCount, configData.yCount), cornerSet);
//                                              break;
//                                      default:
//                                              patternFound = findChessboardCorners(tmpCpy, cvSize(configData.xCount, configData.yCount), cornerSet);
//                                              break;
//                              }

                                //printf("%s << Pattern found? %d\n", __FUNCTION__, patternFound);

                                tmpCpy.copyTo(dispMat);


                                drawChessboardCorners(dispMat, cvSize(configData.xCount, configData.yCount), Mat(cornerSet), patternFound);

                                if (patternFound) {
                                    //HGH
                                    tmpCpy.copyTo(prevMat);

                                        //ROS_WARN("Pattern found!");
                                        pointSets[0].push_back(cornerSet);
                                }
                                
                                std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug_1.data.begin());
                                
                                //printf("%s::%s << Debug copied.\n", __PROGRAM__, __FUNCTION__);
                                
                                debug_pub_1.publish(msg_debug_1, debug_camera_info_1);
                        }
                }
                
        }
        
        frameCount++;
                
}

calibratorNode::calibratorNode(ros::NodeHandle& nh, calibratorData startupData) {
        
        //alpha = 0.00;
        
        alphaChanged = true;
        
        publishCount = 0;
        
        readyForOutput = false;
        
        topValidHeight = 0;
        botValidHeight = 65535;
        
        checkIndex_1 = 0;
        checkIndex_2 = 0;
        
        ref = &nh;
        
        stillCollecting = true;

        //HGH
        stillCollectingSet[0] = true;
        stillCollectingSet[1] = true;
        doVerify = false;
        
        default_R = Mat::eye(3,3,CV_64FC1);
        
        avgTime = 0.0;
        
        numCams = 1;
        
        frameCount = 0;
        frameCount_1 = 0;
        frameCount_2 = 0;
        //HGH
        totalFrameCount_1 = 0;
        totalFrameCount_2 = 0;

        undistortionCount = 0;
        rectificationCount = 0;
        
        infoProcessed = false;
        infoProcessed_1 = false;
        infoProcessed_2 = false;
        
        sprintf(nodeName, "%s", ros::this_node::getName().c_str());
        
        configData = startupData;

        //HGH
        if (configData.patternDetectionMode=="findThenTrack"){
            configData.usePatternDetectionMode = PATTERN_CODE_FIND_TRACK;
            ROS_INFO("Using pattern detection mode: find then track...");
        }else if(configData.patternDetectionMode=="trackThenFind"){
            configData.usePatternDetectionMode = PATTERN_CODE_TRACK_FIND;
            ROS_INFO("Using pattern detection mode: track then find...");
        }else if (configData.patternDetectionMode=="find"){
            configData.usePatternDetectionMode = PATTERN_CODE_FIND;
            ROS_INFO("Using pattern detection mode: find only, no tracking...");
        }else{
            configData.usePatternDetectionMode = PATTERN_CODE_FIND;
            ROS_WARN("patternDetectionMode set to default: find only, no tracking");
        }

        
        if (configData.calibType == "single") {
                
                sprintf(debug_pub_name, "thermalvis%s/image_col", nodeName);
                
                topic = nh.resolveName(configData.video_stream + configData.video_sequence);
                topic_info = nh.resolveName(configData.video_stream + "camera_info");
                
                ROS_INFO("Subscribing to topic (%s) with info (%s).", topic.c_str(), topic_info.c_str());
                
                image_transport::ImageTransport it(nh);
                
                camera_sub = it.subscribeCamera(topic, 1, &calibratorNode::handle_image, this);
                
                /*
                info_sub = nh.subscribe<sensor_msgs::CameraInfo>(topic_info, 1, &calibratorNode::handle_info, this);
                image_sub = it.subscribe(topic, 1, &calibratorNode::handle_image, this);
                */
                if (configData.debugMode) {
                        debug_pub_1 = it.advertiseCamera(debug_pub_name, 1);
                        ROS_INFO("Advertising tracker debugging video (%s)", debug_pub_name);
                }
                
                numCams = 1;
                
        } else if (configData.calibType == "stereo") {
                
                sprintf(debug_pub_name_1, "thermalvis/calib_left/image_col");
                sprintf(debug_pub_name_2, "thermalvis/calib_right/image_col");

                if (configData.useDefinedTopics){

                   topic_1 = nh.resolveName(configData.video_streamLeft + configData.video_sequenceLeft);
                   topic_2 = nh.resolveName(configData.video_streamRight + configData.video_sequenceRight);

                }else{
                //*/
                    topic_1 = nh.resolveName(configData.video_stream + "left" + "/" + configData.video_sequence);
                    topic_2 = nh.resolveName(configData.video_stream + "right" + "/" + configData.video_sequence);
                

                }//HGH

                ROS_INFO("(cam 1): Subscribing to topic (%s)...", topic_1.c_str());
                ROS_INFO("(cam 2): Subscribing to topic (%s)...", topic_2.c_str());
                
                image_transport::ImageTransport it(nh);

                camera_sub_1 = it.subscribeCamera(topic_1, 1, &calibratorNode::handle_image_1, this);
                camera_sub_2 = it.subscribeCamera(topic_2, 1, &calibratorNode::handle_image_2, this);
                
                /*
                info_sub_1 = nh.subscribe<sensor_msgs::CameraInfo>(topic_info_1, 1, &calibratorNode::handle_info_1, this);
                info_sub_2 = nh.subscribe<sensor_msgs::CameraInfo>(topic_info_2, 1, &calibratorNode::handle_info_2, this);
                image_sub_1 = it.subscribe(topic_1, 1, &calibratorNode::handle_image_1, this);
                image_sub_2 = it.subscribe(topic_2, 1, &calibratorNode::handle_image_2, this);
                */
                
                ROS_INFO("Subscribed to topics...");
                
                if (configData.debugMode) {
                        debug_pub_1 = it.advertiseCamera(debug_pub_name_1, 1);
                        debug_pub_2 = it.advertiseCamera(debug_pub_name_2, 1);
                        ROS_INFO("%s << Publishing debug topics at (%s) & (%s)\n", __FUNCTION__, debug_pub_name_1, debug_pub_name_2);
                }
                
                numCams = 2;
                
        }


        
        // Pattern Corner Co-ordinates
    for (unsigned int iii = 0; iii < configData.yCount; iii++) {
        for (unsigned int jjj = 0; jjj < configData.xCount; jjj++) {
            row.push_back(Point3f(((float)iii)*configData.gridSize, ((float)jjj)*configData.gridSize, 0.0));
        }
    }

        ROS_INFO("Establishing server callback...");
        f = boost::bind (&calibratorNode::serverCallback, this, _1, _2);
    server.setCallback (f);
    
}


void calibratorNode::parameterCallback(const sensor_msgs::CameraInfo& msg)
{
        /*
        try     {
                
                configData.cameraData.K = Mat::eye(3, 3, CV_64FC1);
                configData.cameraData.K.at<double>(0,0) = msg.fx();
                configData.cameraData.K.at<double>(1,1) = msg.fy();
                configData.cameraData.K.at<double>(0,2) = msg.cx();
                configData.cameraData.K.at<double>(1,2) = msg.cy();
                
        } catch (sensor_msgs::CvBridgeException& e) {
                ROS_ERROR("Some failure in reading in the camera parameters...");
        }
        */
}

void calibratorNode::serverCallback(thermalvis::calibratorConfig &config, uint32_t level) {
        
  ROS_INFO("Reconfigure Request: (%1.2f, %d)", 
  
            config.alpha,
            config.autoAlpha?1:0);
            
        if ((config.autoAlpha != configData.autoAlpha) || ((!config.autoAlpha) && (config.alpha != configData.alpha))) {
  
                configData.autoAlpha = config.autoAlpha;
                configData.alpha = config.alpha;
                
                alphaChanged = true;
                
                ROS_WARN("About to try and update maps...");
                if (readyForOutput) {
                        ROS_WARN("Updating maps...");
                        
                        for (unsigned int iii = 0; iii < numCams; iii++) {
                                updateIntrinsicMap(iii);
                        }
                        
                }
                
        } else {
                configData.alpha = config.alpha;
        }
        
        configData.trackingMode = config.trackingMode;
        
        ROS_WARN("Tracking mode has been set as (%d)", configData.trackingMode ? 1 : 0);
        
        configData.flowThreshold = config.flowThreshold;
        
        configData.maxFrac = config.maxFrac;

        //HGH
        configData.errorThreshold = config.errorThreshold;

        configData.stopCapturing = config.stopCapturing;

        configData.contrastLeft = config.contrastLeft;
        configData.brightnessLeft = config.brightnessLeft;
        configData.normLeft = config.normLeft;
        configData.invertLeft = config.invertLeft;

        configData.contrastRight = config.contrastRight;
        configData.brightnessRight = config.brightnessRight;
        configData.normRight = config.normRight;
        configData.invertRight = config.invertRight;

        configData.adjustMSERLeft = config.adjustMSERLeft;
        configData.adjustMSERRight = config.adjustMSERRight;
        configData.deltaLeft = config.deltaLeft;
        configData.deltaRight = config.deltaRight;
        configData.deltaLeft = config.deltaLeft;
        configData.maxVarLeft = config.maxVarLeft;
        configData.maxVarRight = config.maxVarRight;
        configData.minDivLeft = config.minDivLeft;
        configData.minDivRight = config.minDivRight;
        configData.areaThresholdLeft = config.areaThresholdLeft;
        configData.areaThresholdRight = config.areaThresholdRight;


}

void calibratorNode::set_ready_for_output() {
        readyForOutput = true;
}

---

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