From ceb00d8d6d7dd6650345e235cbfbd46ac152a9e3 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Mon, 6 May 2019 15:11:38 +0200
Subject: [PATCH] output simplified chain

---
 src/calibration_correction.cpp | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp
index 9fc588c..dee695e 100644
--- a/src/calibration_correction.cpp
+++ b/src/calibration_correction.cpp
@@ -94,6 +94,7 @@ int main(int argc, char* argv[])
   }
   // my_calibration.reset(new Calibration(my_robot));
 
+  /*
   for (auto& calib : calibrations)
   {
     Eigen::Matrix<double, 6, 1> jointvalues;
@@ -150,9 +151,25 @@ int main(int argc, char* argv[])
     // calibration.correctChain();
     // ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties());
   }
-  // my_calibration->correctChain();
+  */
+  my_calibration->correctChain();
+  std::vector<Eigen::Matrix4d> corrected_chain = my_calibration->getSimplified();
+
+  for (auto& it : corrected_chain)
+  {
+    Eigen::Matrix3d rot_a = it.topLeftCorner(3, 3);
+    Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2);
+    ROS_INFO_STREAM("Translation: [" << it.topRightCorner(3, 1).transpose() << "]");
+    ROS_INFO_STREAM("Rotation: [" << rpy_a.transpose() << "]");
+  }
+
+  std::ofstream file;
+  file.open("test.yaml");
+  my_calibration->writeToYaml(file);
+  file.close();
 
   // ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties());
+  std::cout << my_calibration->toXacroProperties() << std::endl;
 
   // tf_listener.reset(new tf::TransformListener);