From e898504ad6a8ea2f23a72d002e2d7604cd382b14 Mon Sep 17 00:00:00 2001
From: aharmat <aharmat@gmail.com>
Date: Tue, 20 Jan 2015 13:21:59 -0500
Subject: [PATCH] Updated VideoSourceSingle to get image size before camera
 info, in case camera info doesn't contain taylor model and we write a default
 set of parameters back

---
 launch/mcptam.launch     |  2 --
 src/VideoSourceSingle.cc | 27 ++++++++++++++-------------
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/launch/mcptam.launch b/launch/mcptam.launch
index ef678fd..969bc4e 100644
--- a/launch/mcptam.launch
+++ b/launch/mcptam.launch
@@ -21,10 +21,8 @@
   <param name="camera_pose_file" type="string" value="$(find mcptam)/poses/poses.dat" />
   -->
 
-  <!--
   <rosparam command="load" file="$(find mcptam)/masks/masks_1,2,3,4.yaml" />
   <param name="masks_dir" type="string" value="$(find mcptam)/masks" />
-  -->
 
   <param name="mm_init_point_mode" type="string" value="$(arg init_mode)" />
   <param name="mm_init_point_max_num" type="int" value="200" />
diff --git a/src/VideoSourceSingle.cc b/src/VideoSourceSingle.cc
index 7410fd8..b9a27b3 100644
--- a/src/VideoSourceSingle.cc
+++ b/src/VideoSourceSingle.cc
@@ -72,6 +72,19 @@ VideoSourceSingle::VideoSourceSingle(bool bGetPoseSeparately)
   
   ROS_INFO("VideoSourceSingle: Set up all subscriptions");
   
+  // Acquire an image and get image size from it
+  mbAcquiredImage = false;
+  while(!mbAcquiredImage && ros::ok())
+  {
+    // Trigger ROS callbacks
+    mCallbackQueue.callAvailable();
+  }
+  
+  ROS_INFO("VideoSourceSingle: Got first image, extracting size");
+  mirSize = CVD::ImageRef(mCVPtr->image.cols,mCVPtr->image.rows);
+  mirFullScaleSize = CVD::ImageRef(mirSize.x * mirBinning.x, mirSize.y * mirBinning.y);
+  
+  // Acquire the calibration info
   mbAcquiredInfo = false;
   while(!mbAcquiredInfo && ros::ok())
   {
@@ -83,6 +96,7 @@ VideoSourceSingle::VideoSourceSingle(bool bGetPoseSeparately)
   mInfoSub.shutdown();  
   ROS_INFO("VideoSourceSingle: Got calibration info");
   
+  // Acquire pose info
   if(mbGetPoseSeparately)
   {
     mbAcquiredPose = false;
@@ -95,19 +109,6 @@ VideoSourceSingle::VideoSourceSingle(bool bGetPoseSeparately)
     mPoseSub.shutdown();
     ROS_INFO("VideoSourceSingle: Got pose");
   }
-  
-  mbAcquiredImage = false;
-  while(!mbAcquiredImage && ros::ok())
-  {
-    // Trigger ROS callbacks
-    mCallbackQueue.callAvailable();
-  }
-  
-  ROS_INFO("VideoSourceSingle: Got first image, extracting size");
-
-  mirSize = CVD::ImageRef(mCVPtr->image.cols,mCVPtr->image.rows);
-  mirFullScaleSize = CVD::ImageRef(mirSize.x * mirBinning.x, mirSize.y * mirBinning.y);
-  
 }
 
 VideoSourceSingle::~VideoSourceSingle()