Setting sweep intermediate cloud optimizer camera parameters from sweep

strands-project · Apr 13, 2016 · 00327df · 00327df
1 parent ded4fa1
commit 00327df
Showing 1 changed file with 13 additions and 1 deletion.
diff --git 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++)
     {