From 00327df8fa1ec978b9346046a4630d965a6e94af Mon Sep 17 00:00:00 2001
From: Rares Ambrus <raambrus@kth.se>
Date: Wed, 13 Apr 2016 18:32:15 +0200
Subject: [PATCH] Setting sweep intermediate cloud optimizer camera parameters
 from sweep

---
 .../src/calibrate_sweep_action_server.cpp          | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
index 022ae1f2..19968523 100644
--- a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
+++ b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp
@@ -107,7 +107,19 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server*
     unsigned int gy = 3;
     unsigned int todoy = 3;
     RobotContainer * rc = new RobotContainer(gx,todox,gy,todoy);
-    rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480);
+
+    // initialize camera parameters from the sweep
+    if (matchingObservations.size()){
+        SemanticRoom<PointType> aRoom = SemanticRoomXMLParser<PointType>::loadRoomFromXML(matchingObservations[0],true);
+        if (aRoom.getIntermediateCloudCameraParameters().size()){
+            image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0];
+            rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height);
+        } else {
+            // no camera parameters saved with the sweep -> initialize optimizer with default parameters
+            rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480);
+        }
+    }
+
 
     for (size_t i=0; i<goal->max_num_sweeps && i<matchingObservations.size(); i++)
     {