thermalvis: thermalvis: depth.cpp Source File

00001 
00005 #include "depth.hpp"
00006 
00007 int main(int argc, char** argv) {
00008         
00009         printf("%s::%s << Node launched.\n", __PROGRAM__, __FUNCTION__);
00010         
00011         ros::init(argc, argv, "depth");
00012         
00013         ros::NodeHandle private_node_handle("~");
00014         
00015         stereoData startupData;
00016         
00017         startupData.read_addr = argv[0];
00018         
00019         
00020         
00021         printf("%s::%s << startupData.read_addr = %s\n", __PROGRAM__, __FUNCTION__, startupData.read_addr.c_str());
00022                 
00023         bool inputIsValid = startupData.obtainStartingData(private_node_handle);
00024         
00025         if (!inputIsValid) {
00026                 printf("%s << Configuration invalid.\n", __FUNCTION__);
00027         }
00028                 
00029         printf("%s::%s << Startup data processed.\n", __PROGRAM__, __FUNCTION__);
00030         
00031         //ros::Rate loop_rate(25);
00032         
00033         
00034         ros::NodeHandle nh;
00035         
00036         stereoDepthNode stereo_node(nh, startupData);
00037         
00038         printf("%s::%s << Node configured.\n", __PROGRAM__, __FUNCTION__);
00039         
00040         ros::spin();
00041         return 0;
00042         
00043 }
00044 
00045 stereoDepthNode::stereoDepthNode(ros::NodeHandle& nh, stereoData startupData) {
00046         
00047         alphaChanged = true;
00048         
00049         configData = startupData;
00050         
00051         
00052         printf("%s << Initializing 'stereoDepthNode'...\n", __FUNCTION__);
00053         
00054         // look for "left" and "right" cameras in video_stream
00055         
00056         std::string topic_info_1 = nh.resolveName(configData.video_stream + "left/camera_info");
00057         std::string topic_info_2 = nh.resolveName(configData.video_stream + "right/camera_info");
00058         
00059         std::string topic_1 = nh.resolveName(configData.video_stream + "left/image_raw");
00060         std::string topic_2 = nh.resolveName(configData.video_stream + "right/image_raw");
00061         
00062         printf("%s << topic_1 = %s; topic_2 = %s\n", __FUNCTION__, topic_1.c_str(), topic_2.c_str());
00063         
00064         image_transport::ImageTransport it(nh);
00065         camera_sub_1 = it.subscribeCamera(topic_1, 1, &stereoDepthNode::handle_image_1, this);
00066         camera_sub_2 = it.subscribeCamera(topic_2, 1, &stereoDepthNode::handle_image_2, this);
00067         
00068         timer = nh.createTimer(ros::Duration(0.01), &stereoDepthNode::timed_loop, this);
00069         
00070         infoProcessed_1 = false;
00071         infoProcessed_2 = false;
00072         
00073         frameCount_1 = 0;
00074         frameCount_2 = 0;
00075         frameCount = 0;
00076         
00077         firstPair = true;
00078         
00079         firstImProcessed_1 = false;
00080         firstImProcessed_2 = false;
00081         firstPairProcessed = false;
00082         
00083         // Set up stereo
00084         
00085         enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2 };
00086         int alg = STEREO_HH; //STEREO_SGBM;
00087                 
00088         SADWindowSize = 5;
00089         numberOfDisparities = 0;
00090         sgbm.preFilterCap = 31; //63;
00091     sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
00092         
00093         
00094     int cn = 1; //img1.channels();
00095 
00096         unsigned int width = 640;
00097     numberOfDisparities = 128;
00098     numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : width/8;
00099     
00100     bm.state->roi1 = roi1;
00101     bm.state->roi2 = roi2;
00102     bm.state->preFilterSize = 41;
00103     bm.state->preFilterCap = 31;
00104     bm.state->SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 41;
00105     bm.state->minDisparity = -64;
00106     bm.state->numberOfDisparities = numberOfDisparities;
00107     bm.state->textureThreshold = 10;
00108     bm.state->uniquenessRatio = 15;
00109     bm.state->speckleWindowSize = 100;
00110     bm.state->speckleRange = 32;
00111     bm.state->disp12MaxDiff = 1;
00112     
00113     sgbm.P1 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
00114     sgbm.P2 = 128*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
00115     sgbm.minDisparity = 0;
00116     sgbm.numberOfDisparities = numberOfDisparities;
00117     sgbm.uniquenessRatio = 10;
00118     sgbm.speckleWindowSize = bm.state->speckleWindowSize;
00119     sgbm.speckleRange = bm.state->speckleRange;
00120     sgbm.disp12MaxDiff = 1;
00121     sgbm.fullDP = alg == STEREO_HH;
00122     
00123     
00124     disparityPublisher = nh.advertise<stereo_msgs::DisparityImage>( "thermalvis/disparity", 1);
00125     
00126     printf("%s << read_addr = %s\n", __FUNCTION__, configData.read_addr.c_str());
00127     printf("%s << extrinsics = %s\n", __FUNCTION__, configData.extrinsics.c_str());
00128 
00129         if ((configData.extrinsics.at(0) == '.') && (configData.extrinsics.at(1) == '.')) {
00130                 configData.read_addr.replace(configData.read_addr.end()-5, configData.read_addr.end(), configData.extrinsics);
00131                 //configData.read_addr = configData.read_addr + "/" + configData.extrinsics;
00132         } else {
00133                 configData.read_addr = configData.extrinsics;
00134         }
00135         
00136         printf("%s << read_addr = %s\n", __FUNCTION__, configData.read_addr.c_str());
00137         
00138         FileStorage fs(configData.read_addr, FileStorage::READ);
00139         fs["R_0"] >> R0;
00140         fs["R_1"] >> R1;
00141         fs["P_0"] >> P0;
00142         fs["P_1"] >> P1;
00143         fs["F"] >> F;
00144         fs["Q1"] >> Q1;
00145         fs["Q2"] >> Q2;
00146         fs["T1"] >> t;
00147         fs.release();
00148         
00149         cout << "R0 = " << R0 << endl;
00150         cout << "R1 = " << R1 << endl;
00151         cout << "t = " << t << endl;
00152         // cout << "P0 = " << P0 << endl;
00153         // cout << "P1 = " << P1 << endl;
00154         
00155         sprintf(cam_pub_name_1, "thermalvis/left/image_rect_color");
00156         sprintf(cam_pub_name_2, "thermalvis/right/image_rect_color");
00157         
00158         cam_pub_1 = it.advertiseCamera(cam_pub_name_1, 1);
00159         cam_pub_2 = it.advertiseCamera(cam_pub_name_2, 1);
00160         
00161         ROS_INFO("Establishing server callback...");
00162         f = boost::bind (&stereoDepthNode::serverCallback, this, _1, _2);
00163     server.setCallback (f);
00164 
00165 }
00166 
00167 bool stereoData::obtainStartingData(ros::NodeHandle& nh) {
00168         
00169         nh.param<std::string>("video_stream", video_stream, "video_stream");
00170         
00171         if (video_stream != "video_stream") {
00172                 printf("%s << Video stream (%s) selected.\n", __FUNCTION__, video_stream.c_str());
00173         } else {
00174                 printf("%s << ERROR! No video stream specified.\n", __FUNCTION__);
00175                 return false;
00176         }
00177         
00178         nh.param<std::string>("extrinsics", extrinsics, "extrinsics");
00179         
00180         printf("%s << Extrinsics at %s selected.\n", __FUNCTION__, extrinsics.c_str());
00181         
00182         
00183         nh.param<bool>("debug_mode", debugMode, false);
00184         
00185         if (debugMode) {
00186                 printf("%s << Running in DEBUG mode.\n", __FUNCTION__);
00187         } else {
00188                 printf("%s << Running in OPTIMIZED mode.\n", __FUNCTION__);
00189         }
00190         
00191         nh.param<bool>("autoAlpha", autoAlpha, false);
00192         
00193         nh.param<double>("alpha", alpha, 0.0);
00194         
00195         maxTimeDiff = 0.02;
00196 
00197         
00198         return true;
00199 }
00200 
00201 void stereoDepthNode::handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
00202         
00203         ROS_INFO("Entered (handle_image_1)...");
00204         
00205         while (!infoProcessed_1) {
00206                 
00207                 printf("%s::%s << Trying to process info (1) ... \n", __PROGRAM__, __FUNCTION__);
00208                 
00209                 try     {
00210                         // Read in camera matrix
00211                         configData.cameraData1.K = Mat::eye(3, 3, CV_64FC1);
00212                         
00213                         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00214                                 for (unsigned int nnn = 0; nnn < 3; nnn++) {
00215                                         configData.cameraData1.K.at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
00216                                 }
00217                         }
00218                         
00219                         cout << configData.cameraData1.K << endl;
00220                         
00221                         configData.cameraData1.cameraSize.width = info_msg->width;
00222                         configData.cameraData1.cameraSize.height = info_msg->height;
00223                         
00224                                                 
00225                         printf("%s << (%d, %d)\n", __FUNCTION__, configData.cameraData1.cameraSize.width, configData.cameraData1.cameraSize.height);
00226                         
00227                         unsigned int maxDistortionIndex;
00228                         if (info_msg->distortion_model == "plumb_bob") {
00229                                 maxDistortionIndex = 5;
00230                         } else if (info_msg->distortion_model == "rational_polynomial") {
00231                                 maxDistortionIndex = 8;
00232                         }
00233                         
00234                         configData.cameraData1.distCoeffs = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
00235                         
00236                         for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
00237                                 configData.cameraData1.distCoeffs.at<double>(0, iii) = info_msg->D[iii];
00238                         }
00239                         
00240                         cout << configData.cameraData1.distCoeffs << endl;
00241                         
00242                         configData.cameraData1.newCamMat = Mat::zeros(3, 3, CV_64FC1);
00243                                         
00244                         
00245                         bool centerPrincipalPoint = false;
00246                         
00247                         
00248                         
00249                         configData.cameraData1.P = Mat::zeros(3, 4, CV_64FC1);
00250                         
00251                         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00252                                 for (unsigned int nnn = 0; nnn < 4; nnn++) {
00253                                         configData.cameraData1.P.at<double>(mmm, nnn) = info_msg->P[4*mmm + nnn];
00254                                         //cout << "P[" << (3*mmm + nnn) << "] = " << info_msg->P[3*mmm + nnn] << endl;
00255                                 }
00256                         }
00257                         
00258                         cout << "P1 = " << configData.cameraData1.P << endl;
00259                         
00260                         /*
00261                         struct cameraParameters {
00262                                 Mat cameraMatrix;
00263                                 Mat distCoeffs;
00264                                 Mat blankCoeffs;
00265                                 Mat newCamMat;
00266                                 Mat imageSize;
00267                                 Size cameraSize;
00268                                 
00269                                 Mat K, K_inv;
00270                                 
00271                                 bool getCameraParameters(string intrinsics);
00272                                 
00273                         };
00274                         */
00275                 
00276                         //ROS_INFO("Initializing map (1)...");
00277                         
00278                         cout << "R0 = " << R0 << endl;
00279                         
00280                         //configData.cameraData1.newCamMat = getOptimalNewCameraMatrix(configData.cameraData1.K, configData.cameraData1.distCoeffs, configData.cameraData1.cameraSize, configData.alpha, configData.cameraData1.cameraSize, &roi1, centerPrincipalPoint);
00281                         
00282                         //cout << configData.cameraData1.newCamMat << endl;
00283                         //initUndistortRectifyMap(configData.cameraData1.K, configData.cameraData1.distCoeffs, R0, configData.cameraData1.newCamMat, configData.cameraData1.cameraSize, CV_32FC1, map11, map12);
00284                         
00285                         //ROS_INFO("Map (1) initialized.");
00286                         
00287                         //assignDebugCameraInfo();
00288                         
00289                         //printf("%s::%s << Debug data assigned.\n", __PROGRAM__, __FUNCTION__);
00290                         
00291                         /*
00292                         msg_debug.width = configData.cameraData1.cameraSize.width; 
00293                         msg_debug.height = configData.cameraData1.cameraSize.height;
00294                         msg_debug.encoding = "bgr8";
00295                         msg_debug.is_bigendian = false;
00296                         msg_debug.step = configData.cameraData1.cameraSize.width*3;
00297                         msg_debug.data.resize(configData.cameraData1.cameraSize.width*configData.cameraData1.cameraSize.height*3);
00298                         */
00299                         
00300                         //minVal = configData.minLimit;
00301                         //maxVal = configData.maxLimit;
00302                         
00303                         infoProcessed_1 = true;
00304                         
00305                 } catch (...) { // (sensor_msgs::CvBridgeException& e) {
00306                         ROS_ERROR("Some failure in reading in the camera parameters...");
00307                 } 
00308                 
00309         }
00310         
00311         // ROS_WARN("Camera (1) : %f", info_msg->header.stamp.toSec());
00312         
00313         cam_info_1 = *info_msg;
00314         
00315         // Update P matrix:
00316         
00317         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00318                 for (unsigned int nnn = 0; nnn < 4; nnn++) {
00319                         //info_msg->P[4*mmm + nnn] = P0.at<double>(mmm, nnn); 
00320                 }
00321         }
00322         
00323         if (infoProcessed_1) {
00324 
00325                 cv_ptr_1 = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                     // For some reason it reads as BGR, not gray
00326 
00327                 //printf("%s << Processing camera 1 image...\n", __FUNCTION__);
00328                 
00329                 Mat newImage(cv_ptr_1->image);
00330                 
00331                 Mat image16, imageDown, greyImX, undistIm;
00332                 cvtColor(newImage, greyImX, CV_RGB2GRAY);
00333                 
00334                 
00335                 unsigned int lastFrameCount = frameCount_1;
00336                 
00337                 
00338                 if (!matricesAreEqual(greyImX, lastImage_1)) {
00339                         
00340                         //printf("%s::%s << New image (%d)\n", __PROGRAM__, __FUNCTION__, frameCount_1);
00341                         
00342                         elapsedTime = timeElapsedMS(cycle_timer);
00343                         
00344                         greyImX.copyTo(grayImageBuffer_1[frameCount_1 % MAXIMUM_FRAMES_TO_STORE]);
00345 
00346                         if (configData.debugMode) {
00347                                 
00348                                 //imshow("image_1", undistIm);
00349                                 //waitKey(1);
00350                                 
00351                         }
00352                         
00353                         lastImage_1.copyTo(olderImage_1);
00354                         greyImX.copyTo(lastImage_1);
00355                         
00356                         time_buffer_1[frameCount_1 % MAXIMUM_FRAMES_TO_STORE] = info_msg->header.stamp;
00357                         frameCount_1++;
00358                         
00359                         if (_access.try_lock()) {
00360                                 updatePairs();
00361                                 _access.unlock();
00362                         }
00363                         
00364                         if (!firstImProcessed_1) {
00365                                 firstImProcessed_1 = true;
00366                         }
00367                         
00368                         if (firstImProcessed_1 && firstImProcessed_2) {
00369                                 firstPairProcessed = true;
00370                         }
00371 
00372                 } else {
00373                         //printf("%s::%s << Matrices are equal.\n", __PROGRAM__, __FUNCTION__);
00374                         elapsedTime = timeElapsedMS(cycle_timer, false);
00375                 }
00376         }
00377 
00378 }
00379 
00380 void stereoDepthNode::handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
00381                         
00382         while (!infoProcessed_2) {
00383                 
00384                 printf("%s::%s << Trying to process info (1) ... \n", __PROGRAM__, __FUNCTION__);
00385                 
00386                 try     {
00387                         // Read in camera matrix
00388                         configData.cameraData2.K = Mat::eye(3, 3, CV_64FC1);
00389                         
00390                         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00391                                 for (unsigned int nnn = 0; nnn < 3; nnn++) {
00392                                         configData.cameraData2.K.at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
00393                                 }
00394                         }
00395                         
00396                         cout << configData.cameraData2.K << endl;
00397                         
00398                         configData.cameraData2.cameraSize.width = info_msg->width;
00399                         configData.cameraData2.cameraSize.height = info_msg->height;
00400                         
00401                                                 
00402                         printf("%s << (%d, %d)\n", __FUNCTION__, configData.cameraData2.cameraSize.width, configData.cameraData2.cameraSize.height);
00403                         
00404                         unsigned int maxDistortionIndex;
00405                         if (info_msg->distortion_model == "plumb_bob") {
00406                                 maxDistortionIndex = 5;
00407                         } else if (info_msg->distortion_model == "rational_polynomial") {
00408                                 maxDistortionIndex = 8;
00409                         }
00410                         
00411                         configData.cameraData2.distCoeffs = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
00412                         
00413                         for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
00414                                 configData.cameraData2.distCoeffs.at<double>(0, iii) = info_msg->D[iii];
00415                         }
00416                         
00417                         cout << configData.cameraData2.distCoeffs << endl;
00418                         
00419                         configData.cameraData2.newCamMat = Mat::zeros(3, 3, CV_64FC1);
00420                         
00421                         //Rect* validPixROI = 0;
00422                         
00423                         bool centerPrincipalPoint = true;
00424                         
00425                         
00426                         
00427                         configData.cameraData2.P = Mat::zeros(3, 4, CV_64FC1);
00428                         
00429                         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00430                                 for (unsigned int nnn = 0; nnn < 4; nnn++) {
00431                                         configData.cameraData2.P.at<double>(mmm, nnn) = info_msg->P[4*mmm + nnn];
00432                                 }
00433                         }
00434                         
00435                         cout << "P2 = " << configData.cameraData2.P << endl;
00436                         
00437                         /*
00438                         struct cameraParameters {
00439                                 Mat cameraMatrix;
00440                                 Mat distCoeffs;
00441                                 Mat blankCoeffs;
00442                                 Mat newCamMat;
00443                                 Mat imageSize;
00444                                 Size cameraSize;
00445                                 
00446                                 Mat K, K_inv;
00447                                 
00448                                 bool getCameraParameters(string intrinsics);
00449                                 
00450                         };
00451                         */
00452                 
00453                         //ROS_INFO("Initializing map (2)...");
00454                         
00455                         //configData.cameraData2.newCamMat = getOptimalNewCameraMatrix(configData.cameraData2.K, configData.cameraData2.distCoeffs, configData.cameraData2.cameraSize, configData.alpha, configData.cameraData2.cameraSize, &roi2, centerPrincipalPoint);
00456                         
00457                         //cout << configData.cameraData2.newCamMat << endl;
00458                         
00459                         //initUndistortRectifyMap(configData.cameraData2.K, configData.cameraData2.distCoeffs, R1, configData.cameraData2.newCamMat, configData.cameraData2.cameraSize, CV_32FC1, map21, map22);
00460                         
00461                         //ROS_INFO("Map (2) initialized.");
00462                         
00463                         //assignDebugCameraInfo();
00464                         
00465                         //printf("%s::%s << Debug data assigned.\n", __PROGRAM__, __FUNCTION__);
00466                         
00467                         /*
00468                         msg_debug.width = configData.cameraData2.cameraSize.width; 
00469                         msg_debug.height = configData.cameraData2.cameraSize.height;
00470                         msg_debug.encoding = "bgr8";
00471                         msg_debug.is_bigendian = false;
00472                         msg_debug.step = configData.cameraData2.cameraSize.width*3;
00473                         msg_debug.data.resize(configData.cameraData2.cameraSize.width*configData.cameraData2.cameraSize.height*3);
00474                         */
00475                         
00476                         //minVal = configData.minLimit;
00477                         //maxVal = configData.maxLimit;
00478                         
00479                         infoProcessed_2 = true;
00480                         
00481                 } catch (...) { // (sensor_msgs::CvBridgeException& e) {
00482                         ROS_ERROR("Some failure in reading in the camera parameters...");
00483                 }
00484                 
00485         }
00486         
00487         // ROS_WARN("Camera (2) : %f", info_msg->header.stamp.toSec());
00488         
00489         cam_info_2 = *info_msg;
00490         
00491         for (unsigned int mmm = 0; mmm < 3; mmm++) {
00492                 for (unsigned int nnn = 0; nnn < 4; nnn++) {
00493                         //info_msg->P[4*mmm + nnn] = P1.at<double>(mmm, nnn); 
00494                 }
00495         }
00496         
00497         if (infoProcessed_2) {
00498 
00499                 cv_ptr_2 = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                     // For some reason it reads as BGR, not gray
00500 
00501                 //printf("%s << Processing camera 2 image...\n", __FUNCTION__);
00502 
00503                 Mat newImage(cv_ptr_2->image);
00504                 
00505                 Mat image16, imageDown, greyImX, undistIm;
00506                 cvtColor(newImage, greyImX, CV_RGB2GRAY);
00507                 
00508                 unsigned int lastFrameCount = frameCount_2;
00509                 
00510                 if (!matricesAreEqual(greyImX, lastImage_2)) {
00511                         
00512                         //printf("%s::%s << New image (%d)\n", __PROGRAM__, __FUNCTION__, frameCount_2);
00513                         
00514                         elapsedTime = timeElapsedMS(cycle_timer);
00515                         
00516                         greyImX.copyTo(grayImageBuffer_2[frameCount_2 % MAXIMUM_FRAMES_TO_STORE]);
00517 
00518                         if (configData.debugMode) {
00519                                 
00520                                 //imshow("image_2", undistIm);
00521                                 //waitKey(1);
00522                                 
00523                         }
00524                         
00525                         lastImage_2.copyTo(olderImage_2);
00526                         greyImX.copyTo(lastImage_2);
00527                         
00528                         time_buffer_2[frameCount_2 % MAXIMUM_FRAMES_TO_STORE] = info_msg->header.stamp;
00529                         frameCount_2++;
00530                         
00531                         if (_access.try_lock()) {
00532                                 updatePairs();
00533                                 _access.unlock();
00534                         }
00535                         
00536                         if (!firstImProcessed_2) {
00537                                 firstImProcessed_2 = true;
00538                         }
00539                         
00540                         if (firstImProcessed_1 && firstImProcessed_2) {
00541                                 firstPairProcessed = true;
00542                         }
00543 
00544                 } else {
00545                         printf("%s::%s << Matrices are equal.\n", __PROGRAM__, __FUNCTION__);
00546                         elapsedTime = timeElapsedMS(cycle_timer, false);
00547                 }
00548                 
00549         }
00550 
00551 }
00552 
00553 void stereoDepthNode::updatePairs() {
00554         
00555         //ROS_INFO("Entered (updatePairs)...");
00556         
00557         int maxCam1 = frameCount_1;
00558         int maxCam2 = frameCount_2;
00559         
00560         if ((checkIndex_1 >= maxCam1) && (checkIndex_2 >= maxCam2)) {
00561                 return;
00562         }
00563         
00564         if ((maxCam1 == 0) || (maxCam2 == 0)) {
00565                 return;
00566         }
00567         
00568         //ROS_INFO("Continuing (updatePairs)...");
00569         
00570         ros::Time time_1 = time_buffer_1[(maxCam1-1) % MAXIMUM_FRAMES_TO_STORE];
00571         ros::Time time_2 = time_buffer_2[(maxCam2-1) % MAXIMUM_FRAMES_TO_STORE];
00572         
00573         // ROS_INFO("maxCam1 = (%d); maxCam2 = (%d) - [%d.%d] [%d.%d]", maxCam1, maxCam2, time_1.sec, time_1.nsec, time_2.sec, time_2.nsec);
00574         
00575         while (checkIndex_1 < maxCam1) {
00576                 
00577                 //ROS_INFO("Looping (1)...");
00578                 
00579                 /*
00580                 if  (duplicateFlags_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE] == 1) {
00581                         checkIndex_1++;
00582                         continue;
00583                 }
00584                 */
00585                 
00586                 double leastDiff = 9e99;
00587                 int bestMatch = -1;
00588                 
00589                 bool candidatesExhausted = false;
00590         
00591                 while ((timeDiff(time_buffer_2[checkIndex_2 % MAXIMUM_FRAMES_TO_STORE], time_buffer_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE]) < configData.maxTimeDiff)) {
00592                         
00593                         //ROS_INFO("Looping (2)...");
00594                         
00595                         if (checkIndex_2 >= (maxCam2-1)) {
00596                                 break;
00597                                 
00598                         }
00599                         
00600                         /*
00601                         if  (duplicateFlags_2[checkIndex_2 % MAXIMUM_FRAMES_TO_STORE] == 1) {
00602                                 checkIndex_2++;
00603                                 continue;
00604                         }
00605                         */
00606                         
00607                         double diff = abs(timeDiff(time_buffer_2[checkIndex_2 % MAXIMUM_FRAMES_TO_STORE], time_buffer_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE]));
00608                         
00609                         if (diff < leastDiff) {
00610                                 leastDiff = diff;
00611                                 bestMatch = checkIndex_2;
00612                         }
00613                         
00614                         checkIndex_2++;
00615                 }
00616                 
00617                 if ((timeDiff(time_buffer_2[checkIndex_2 % MAXIMUM_FRAMES_TO_STORE], time_buffer_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE]) >= configData.maxTimeDiff)) {
00618                         candidatesExhausted = true;
00619                 }
00620                 
00621                 //checkIndex_2 = bestMatch;
00622                 
00623                 if ((leastDiff < configData.maxTimeDiff) && (bestMatch >= 0)) {
00624                         
00625                         //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);
00626                         
00627                         ROS_WARN("Adding pair (%d) & (%d) (%f) [%d.%d] & [%d.%d]", checkIndex_1, bestMatch, leastDiff, time_buffer_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE].sec, time_buffer_1[checkIndex_1 % MAXIMUM_FRAMES_TO_STORE].nsec, time_buffer_2[bestMatch % MAXIMUM_FRAMES_TO_STORE].sec, time_buffer_2[bestMatch % MAXIMUM_FRAMES_TO_STORE].nsec);
00628                         
00629                         validPairs[0].push_back(checkIndex_1);
00630                         checkIndex_1++;
00631                         validPairs[1].push_back(bestMatch);
00632                         
00633                 } else if (candidatesExhausted) { // (checkIndex_2 < (maxCam2-1)) {
00634                         checkIndex_1++;
00635                 } else {
00636                         break;
00637                 }
00638                 
00639         }
00640         
00641         //ROS_INFO("Leaving (updatePairs)...");
00642         
00643 }
00644 
00645 void stereoDepthNode::timed_loop(const ros::TimerEvent& event) {
00646         
00647         //ROS_INFO("Entered (timed_loop)...");
00648         
00649         if (pairsProcessed == validPairs[1].size()) {
00650                 return;
00651         }
00652         
00653         if (infoProcessed_1 && infoProcessed_2) {
00654                 if (alphaChanged) {
00655                         updateMaps();
00656                 }
00657         }
00658         
00659         //printf("%s::%s << Entered...\n", __PROGRAM__, __FUNCTION__);
00660         
00661         stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared<stereo_msgs::DisparityImage>();
00662         
00663         if (firstPairProcessed) {
00664                 
00665                 // printf("%s::%s << 
00666                 
00667                 //stereo_model.fromCameraInfo(cam_info_1, cam_info_2);
00668                 
00669                 
00670                 
00671                 // Prepare disparity message
00672                 disp_msg->header = cam_info_1.header;
00673                 disp_msg->image.header = cam_info_1.header;
00674                 disp_msg->image.height = cam_info_1.height;
00675                 disp_msg->image.width = cam_info_1.width;
00676                 disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00677                 disp_msg->image.step = disp_msg->image.width * sizeof(float);
00678                 disp_msg->image.data.resize(disp_msg->image.height * disp_msg->image.step);
00679                 
00680                 int border = sgbm.SADWindowSize / 2;
00681                 int left   = sgbm.numberOfDisparities + sgbm.minDisparity + border - 1;
00682                 int wtf = (sgbm.minDisparity >= 0) ? border + sgbm.minDisparity : std::max(border, -sgbm.minDisparity);
00683                 int right  = disp_msg->image.width - 1 - wtf;
00684                 int top    = border;
00685                 int bottom = disp_msg->image.height - 1 - border;
00686                 
00687                 disp_msg->valid_window.x_offset = left;
00688                 disp_msg->valid_window.y_offset = top;
00689                 disp_msg->valid_window.width    = right - left;
00690                 disp_msg->valid_window.height   = bottom - top;
00691                 
00692                 disp_msg->min_disparity = sgbm.minDisparity;
00693                 disp_msg->max_disparity = sgbm.minDisparity + sgbm.numberOfDisparities - 1;
00694                 disp_msg->delta_d = 1.0 / 16;
00695                 
00696                 //disp_msg->f = stereo_model.right().fx();
00697                 
00698                 disp_msg->f = P0.at<double>(0,0);
00699                 // disp_msg->T = stereo_model.baseline();
00700                 
00701                 disp_msg->T = P0.at<double>(0,3) - P1.at<double>(0,3);
00702                 
00703                 
00704                 
00705                 //firstPair = false;
00706                 
00707                 cv::Mat_<float> disp_image(disp_msg->image.height, disp_msg->image.width, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
00708                 //printf("%s << Processing image pair: (%d, %d)\n", __FUNCTION__, frameCount_1, frameCount_2);
00709         
00710                 elapsedTime = timeElapsedMS(cycle_timer, false);
00711                 
00712                 // Mat imCpy1, imCpy2;
00713                 
00714                 
00715                 
00716                 // Mat image16_1, image16_2, imageDown_1, imageDown_2, undistIm;
00717                 
00718                 //normalize_16(image16_1, grayImageBuffer_1[(frameCount_1-1) % MAXIMUM_FRAMES_TO_STORE], minVal, maxVal);
00719                 //down_level(imageDown_1, image16_1);
00720                 
00721         
00722         //normalize_16(image16_2, grayImageBuffer_2[(frameCount_2-1) % MAXIMUM_FRAMES_TO_STORE], minVal, maxVal);
00723                 //down_level(imageDown_2, image16_2);
00724                 
00725         
00726         double grad, shift;
00727         
00728         Mat _8bit1, _8bit2;
00729         
00730         //relevelImages(grayImageBuffer_1[(frameCount_1-1) % MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[(frameCount_2-1) % MAXIMUM_FRAMES_TO_STORE], disp16, Q, _8bit1, _8bit2, map11, map12, map21, map22);
00731         
00732         ROS_INFO("About to relevel...");
00733         relevelImages(grayImageBuffer_1[(validPairs[0].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[(validPairs[1].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE], disp16, R0, R1, t, Q1, Q2, _8bit1, _8bit2, map11, map12, map21, map22);
00734         
00735         
00736         /*
00737         adaptiveDownsample(grayImageBuffer_1[(frameCount_1-1) % MAXIMUM_FRAMES_TO_STORE], _8bit1);
00738         adaptiveDownsample(grayImageBuffer_2[(frameCount_2-1) % MAXIMUM_FRAMES_TO_STORE], _8bit2);
00739         
00740         remap(_8bit1, undistIm, map11, map12, INTER_LINEAR);
00741                 GaussianBlur(undistIm, imCpy1, Size(7,7), 0.5, 0.5);
00742                 
00743                 remap(_8bit2, undistIm, map21, map22, INTER_LINEAR);
00744         GaussianBlur(undistIm, imCpy2, Size(7,7), 0.5, 0.5);
00745         */
00746         //findRadiometricMapping(grayImageBuffer_1[(frameCount_1-1) % MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[(frameCount_2-1) % MAXIMUM_FRAMES_TO_STORE], grad, shift, imCpy1, imCpy2);
00747         
00748         // ROS_INFO("Radiometric mapping: (%f) (%f)", grad, shift);
00749         
00750         
00751                 
00752                 
00753                 
00754                 /*
00755                 imshow("disparity1", image16_1);
00756                 waitKey(1);
00757                 
00758                 imshow("disparity2", image16_2);
00759                 waitKey(1);
00760                 */
00761                 
00762                 sgbm(_8bit1, _8bit2, disp16);
00763                 
00764                 // filterSpeckles(disp16, 1.0, 5, 16.0);
00765                 
00766                 disp16.convertTo(disp_image, CV_32F);
00767                 
00768                 //ros::Time currTime = ros::Time::now();
00769                 
00770                 //ROS_INFO("Publishing disparity message...");
00771                 
00772                 // This time should actually be the average of the two times corresponding to the frame pair...
00773                 //disp_msg->header.stamp = currTime;
00774                 
00775                 ros::Time currTime = findAverageTime(time_buffer_1[(validPairs[0].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE], time_buffer_2[(validPairs[1].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE]);
00776                 disp_msg->header.stamp = currTime;
00777                 
00778                 ROS_ERROR("AveTime = (%d.%d); time_1 = (%d.%d); time_2 = (%d.%d)", currTime.sec, currTime.nsec, time_buffer_1[(validPairs[0].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE].sec, time_buffer_1[(validPairs[0].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE].nsec, time_buffer_2[(validPairs[1].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE].sec, time_buffer_2[(validPairs[1].at(pairsProcessed)) % MAXIMUM_FRAMES_TO_STORE].nsec);
00779                 
00780                 disparityPublisher.publish(disp_msg);
00781                 
00782                 Mat cam1mat, cam2mat;
00783                 
00784                 cvtColor(_8bit1, cam1mat, CV_GRAY2RGB);
00785                 cvtColor(_8bit2, cam2mat, CV_GRAY2RGB);
00786                 
00787                 vector<vector<vector<Point> > > c1, c2;
00788                 
00789                 getContours(_8bit1, c1);
00790                 getContours(_8bit2, c2);
00791                 
00792                 Mat contourDisp(480, 640, CV_16UC1);
00793                 
00794                 depthFromContours(c1, c2, contourDisp);
00795                 
00796                 //imshow("contourDisp", contourDisp);
00797                 //waitKey(1);
00798                 
00799                 printf("%s << contours: (%d, %d)\n", __FUNCTION__, c1.size(), c2.size());
00800                 
00801                 //drawContours(_8bit1, cam1mat);
00802                 //drawContours(_8bit2, cam2mat);
00803                 //drawEpipolarLines(cam1mat, cam2mat, F);
00804                 
00805                 
00806                 
00807                 if (msg_1.width == 0) {
00808                         msg_1.width = cam1mat.cols; 
00809                         msg_1.height = cam1mat.rows;
00810                         msg_1.encoding = "bgr8";
00811                         msg_1.is_bigendian = false;
00812                         msg_1.step = cam1mat.cols*3;
00813                         msg_1.data.resize(cam1mat.cols*cam1mat.rows*cam1mat.channels());
00814                 }
00815                 
00816                 if (msg_2.width == 0) {
00817                         msg_2.width = cam2mat.cols; 
00818                         msg_2.height = cam2mat.rows;
00819                         msg_2.encoding = "bgr8";
00820                         msg_2.is_bigendian = false;
00821                         msg_2.step = cam2mat.cols*3;
00822                         msg_2.data.resize(cam2mat.cols*cam2mat.rows*cam2mat.channels());
00823                 }
00824                 
00825                 //ROS_INFO("Publishing rectified images...");
00826                 msg_1.header.stamp = currTime;
00827                 camera_info_1.header.stamp = currTime;
00828                 std::copy(&(cam1mat.at<Vec3b>(0,0)[0]), &(cam1mat.at<Vec3b>(0,0)[0])+(cam1mat.cols*cam1mat.rows*cam1mat.channels()), msg_1.data.begin());
00829                 cam_pub_1.publish(msg_1, camera_info_1);
00830                 
00831                 msg_2.header.stamp = currTime;
00832                 camera_info_2.header.stamp = currTime;
00833                 std::copy(&(cam2mat.at<Vec3b>(0,0)[0]), &(cam2mat.at<Vec3b>(0,0)[0])+(cam2mat.cols*cam2mat.rows*cam2mat.channels()), msg_2.data.begin());
00834                 cam_pub_2.publish(msg_2, camera_info_2);
00835                 
00836                 //ROS_INFO("Rectified images published.");
00837                 
00838                 /*
00839                 disp16.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
00840                 resize(disp8, disparityImage, cvSize(640, 480));
00841                 imshow("disparityImage", disparityImage);
00842                 waitKey(1);
00843                 */
00844                 
00845                 //publishMaps();
00846                 
00847                 pairsProcessed++;
00848         }
00849         
00850         
00851         
00852         
00853 }
00854 
00855 void stereoDepthNode::publishMaps() {
00856         
00857         if (disp.rows > 0) {
00858                 
00859                 //disp.convertTo(dispFloat, CV_32F);
00860                 
00861                 //dispFloat /= 16.0; 
00862                 
00863                 if (0) { // (firstPair) {
00864         
00865                         //disp_msg->header         = l_info_msg->header;
00866                         //disp_msg->image.header   = l_info_msg->header;
00867                         /*
00868                         disp_msg->image.height   = disp.rows; // disp_msg->image.height   = l_image_msg->height;
00869                         disp_msg->image.width    = disp.cols;
00870                 
00871                         disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00872                         disp_msg->image.step     = disp_msg->image.width * sizeof(float);
00873                         disp_msg->image.data.resize(disp_msg->image.height * disp_msg->image.step);
00874                         
00875                         disp_msg->delta_d = 1.0 / 16;
00876                         */
00877                         /*
00878                         printf("%s::%s << Setting up disparity outputs...\n", __PROGRAM__, __FUNCTION__);
00879                         disp_image.height = dispFloat.rows;
00880                         disp_image.width = dispFloat.cols;
00881                         disp_image.encoding = "float32";
00882                         disp_image.is_bigendian = false;
00883                         disp_image.step = dispFloat.cols*sizeof(float);
00884                         disp_image.data.resize(dispFloat.cols*dispFloat.rows*sizeof(float));
00885                         
00886                         disp_object.image = disp_image;
00887                         */
00888                         
00889                         
00890                         // disp_object.f = ...;
00891                         // disp_object.T = ...;
00892                         // disp_object.valid_window = ...;
00893                         // disp_object.min_disparity = ...;
00894                         // disp_object.max_disparity = ...;
00895                         // disp_object.delta_d = ...;
00896                         
00897                         firstPair = false;
00898         
00899         
00900         
00901                 }
00902         }
00903         
00904                 
00905         
00906         if (!firstPair) {
00907                 
00908                 printf("%s::%s << Entering publishing segment...\n", __PROGRAM__, __FUNCTION__);
00909                 
00910                 
00911                 //cv::Mat_<float> disp_image(disp_msg->image.height, disp_msg->image.width, reinterpret_cast<float*>(&disp_msg->image.data[0]), disp_msg->image.step);
00912                 
00913                 // disp_image.header
00914                 // disp_image.data
00915                 
00916                 //std::copy(&(disparityImage.at<float>(0,0)), &(disparityImage.at<float>(0,0))+(disparityImage.cols*disparityImage.rows*3), disp_image.data.begin());
00917                 
00918                 //std::copy(&(dispFloat.at<float>(0,0)), &(dispFloat.at<float>(0,0))+(dispFloat.cols*dispFloat.rows*sizeof(float)), disp_image.data.begin());
00919                 
00920                 //disp_object.image = disp_image;
00921                 
00922                 /*
00923                 unsigned int bytesCopied = 0;
00924                 float val = 0.0;
00925                 for (unsigned int iii = 0; iii < disp.rows; iii++) {
00926                         for (unsigned int jjj = 0; jjj < disp.cols; jjj++) {
00927                                 val = ((float) disparityImage.at<unsigned short>(iii,jjj)) / 16.0;
00928                                 std::copy(&val, &val + 4, disp_object.image.data.begin() + bytesCopied);
00929                                 bytesCopied += 4;
00930                         }
00931                 }
00932                 */
00933                 //std::copy(&(disparityImage.at<float>(0,0)), &(disparityImage.at<float>(0,0))+(disparityImage.cols*disparityImage.rows*3), disp_image.data.begin());
00934                 
00935                 printf("%s::%s << disp_object info: (%d, %d)\n", __PROGRAM__, __FUNCTION__, disp_object.image.height, disp_object.image.width);
00936                 
00937                 //disp_object.image.height = 480;
00938                 //disp_object.image.width = 640;
00939                 // disp_object.header = ...
00940                 
00941                 
00942                 
00943                 // disparityImage
00944                 // std::copy(&(newImage.at<Vec3b>(0,0)[0]), &(newImage.at<Vec3b>(0,0)[0])+(newImage.cols*newImage.rows*3), msg_color.data.begin());
00945                                 
00946                 printf("%s::%s << Publishing depth map...\n", __PROGRAM__, __FUNCTION__);
00947                 //imshow("disp", disp);
00948                 //waitKey(1);
00949                 //disparityPublisher.publish(disp_msg);
00950                 //pub_color.publish(msg_color, camera_info);
00951         }
00952         
00953         
00954         
00955 }
00956 
00957 void stereoDepthNode::updateMaps() {
00958         
00959         ROS_INFO("Updating maps...");
00960         
00961         if (configData.autoAlpha) {
00962                 configData.alpha = findBestAlpha(configData.cameraData1.K, configData.cameraData2.K, configData.cameraData1.distCoeffs, configData.cameraData2.distCoeffs, configData.cameraData1.cameraSize, R0, R1);
00963         }
00964         
00965         Rect* validPixROI = 0;
00966         bool centerPrincipalPoint = true;
00967         
00968         configData.cameraData1.newCamMat = getOptimalNewCameraMatrix(configData.cameraData1.K, configData.cameraData1.distCoeffs, configData.cameraData1.cameraSize, configData.alpha, configData.cameraData1.cameraSize, &roi1, centerPrincipalPoint);
00969         configData.cameraData2.newCamMat = getOptimalNewCameraMatrix(configData.cameraData2.K, configData.cameraData2.distCoeffs, configData.cameraData2.cameraSize, configData.alpha, configData.cameraData2.cameraSize, &roi2, centerPrincipalPoint);
00970         
00971         ROS_WARN("Adjusting undistortion mappings...");
00972         initUndistortRectifyMap(configData.cameraData1.K, configData.cameraData1.distCoeffs, R0, configData.cameraData1.newCamMat, configData.cameraData1.cameraSize, CV_32FC1, map11, map12);
00973         initUndistortRectifyMap(configData.cameraData2.K, configData.cameraData2.distCoeffs, R1, configData.cameraData2.newCamMat, configData.cameraData2.cameraSize, CV_32FC1, map21, map22);  
00974         
00975         alphaChanged = false;
00976         
00977 }
00978 
00979 
00980 void stereoDepthNode::serverCallback(thermalvis::depthConfig &config, uint32_t level) {
00981         
00982   ROS_INFO("Reconfigure Request: (%1.2f | [%d])", 
00983         config.alpha,
00984         config.autoAlpha);
00985 
00986         
00987         if ((config.autoAlpha != configData.autoAlpha) || ((!config.autoAlpha) && (config.alpha != configData.alpha))) {
00988   
00989                 configData.autoAlpha = config.autoAlpha;
00990                 configData.alpha = config.alpha;
00991                 
00992                 alphaChanged = true;
00993                 
00994                 ROS_WARN("About to try and update maps...");
00995                 if (infoProcessed_1 && infoProcessed_2) {
00996                         ROS_WARN("Updating maps...");
00997                         updateMaps();
00998                 }
00999                 
01000         } else {
01001                 configData.alpha = config.alpha;
01002         }
01003         
01004       
01005 
01006 }