Updated VideoSourceSingle to get image size before camera info, in ca…

…se camera info doesn't contain taylor model and we write a default set of parameters back

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" />

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()