From 4ea722004a6c23f8481b68c573f3eeb55fec8aa6 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Tue, 2 Aug 2016 17:42:48 +0200
Subject: [PATCH] Fixed parsing to use Transforms for calibration as well.

---
 kitti_to_rosbag/CMakeLists.txt                |  1 +
 .../include/kitti_to_rosbag/kitti_common.h    | 45 +++++++++++++++++++
 .../include/kitti_to_rosbag/kitti_parser.h    | 37 +--------------
 kitti_to_rosbag/src/kitti_parser.cpp          |  5 ++-
 4 files changed, 51 insertions(+), 37 deletions(-)
 create mode 100644 kitti_to_rosbag/include/kitti_to_rosbag/kitti_common.h

diff --git a/kitti_to_rosbag/CMakeLists.txt b/kitti_to_rosbag/CMakeLists.txt
index c270707..9ad30a2 100644
--- a/kitti_to_rosbag/CMakeLists.txt
+++ b/kitti_to_rosbag/CMakeLists.txt
@@ -12,6 +12,7 @@ add_definitions(-std=c++11)
 #############
 cs_add_library(${PROJECT_NAME}
   src/kitti_parser.cpp
+  src/kitti_ros_conversions.cpp
 )
 
 ############
diff --git a/kitti_to_rosbag/include/kitti_to_rosbag/kitti_common.h b/kitti_to_rosbag/include/kitti_to_rosbag/kitti_common.h
new file mode 100644
index 0000000..00dace7
--- /dev/null
+++ b/kitti_to_rosbag/include/kitti_to_rosbag/kitti_common.h
@@ -0,0 +1,45 @@
+#ifndef KITTI_TO_ROSBAG_KITTI_COMMON_H_
+#define KITTI_TO_ROSBAG_KITTI_COMMON_H_
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <kindr/minimal/quat-transformation.h>
+
+namespace kitti {
+
+// Transformation type for defining sensor orientation.
+typedef kindr::minimal::QuatTransformation Transformation;
+typedef kindr::minimal::RotationQuaternion Rotation;
+
+struct CameraCalibration {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Intrinsics.
+  Eigen::Vector2d image_size;                  // S_xx in calibration.
+  Eigen::Matrix3d rect_mat;                    // R_rect_xx in calibration.
+  Eigen::Matrix<double, 3, 4> projection_mat;  // P_xx in calibration.
+
+  // Unrectified (raw) intrinsics. Should only be used if rectified set to
+  // false.
+  Eigen::Matrix3d K;              // Camera intrinsics, K_xx in calibration.
+  Eigen::Matrix<double, 1, 5> D;  // Distortion parameters, radtan model.
+
+  // Extrinsics.
+  Transformation T_cam0_cam;
+  //Eigen::Matrix3d R;  // Rotation matrix, extrinsics, wrt 0th camera (?)
+  //Eigen::Vector3d T;  // Translation matrix, extrinsics, wrt 0th camera (?)
+
+  // Accessor for full transform. Derived from 'raw' extrinsics above.
+  //Transformation getTransformToCam0() const {
+  //  Transformation transform(Rotation::fromApproximateRotationMatrix(R), T);
+  //  return transform;
+  //}
+};
+
+typedef std::vector<CameraCalibration,
+                    Eigen::aligned_allocator<CameraCalibration> >
+    CameraCalibrationVector;
+
+}  // namespace kitti
+
+#endif  // KITTI_TO_ROSBAG_KITTI_COMMON_H_
diff --git a/kitti_to_rosbag/include/kitti_to_rosbag/kitti_parser.h b/kitti_to_rosbag/include/kitti_to_rosbag/kitti_parser.h
index 89db208..9047563 100644
--- a/kitti_to_rosbag/include/kitti_to_rosbag/kitti_parser.h
+++ b/kitti_to_rosbag/include/kitti_to_rosbag/kitti_parser.h
@@ -4,46 +4,13 @@
 #include <memory>
 #include <map>
 
-#include <Eigen/Core>
-#include <Eigen/StdVector>
-#include <kindr/minimal/quat-transformation.h>
 #include <pcl/point_types.h>
 #include <pcl/conversions.h>
 #include <opencv2/core/core.hpp>
 
-namespace kitti {
-
-// Transformation type for defining sensor orientation.
-typedef kindr::minimal::QuatTransformation Transformation;
-typedef kindr::minimal::RotationQuaternion Rotation;
-
-struct CameraCalibration {
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  // Intrinsics.
-  Eigen::Vector2d image_size;                  // S_xx in calibration.
-  Eigen::Matrix3d rect_mat;                    // R_rect_xx in calibration.
-  Eigen::Matrix<double, 3, 4> projection_mat;  // P_xx in calibration.
-
-  // Unrectified (raw) intrinsics. Should only be used if rectified set to
-  // false.
-  Eigen::Matrix3d K;              // Camera intrinsics, K_xx in calibration.
-  Eigen::Matrix<double, 1, 5> D;  // Distortion parameters, radtan model.
+#include "kitti_to_rosbag/kitti_common.h"
 
-  // Extrinsics.
-  Eigen::Matrix3d R;  // Rotation matrix, extrinsics, wrt 0th camera (?)
-  Eigen::Vector3d T;  // Translation matrix, extrinsics, wrt 0th camera (?)
-
-  // Accessor for full transform. Derived from 'raw' extrinsics above.
-  Transformation getTransformToCam0() const {
-    Transformation transform(Rotation::fromApproximateRotationMatrix(R), T);
-    return transform;
-  }
-};
-
-typedef std::vector<CameraCalibration,
-                    Eigen::aligned_allocator<CameraCalibration> >
-    CameraCalibrationVector;
+namespace kitti {
 
 class KittiParser {
  public:
diff --git a/kitti_to_rosbag/src/kitti_parser.cpp b/kitti_to_rosbag/src/kitti_parser.cpp
index bc5c366..17304e7 100644
--- a/kitti_to_rosbag/src/kitti_parser.cpp
+++ b/kitti_to_rosbag/src/kitti_parser.cpp
@@ -260,7 +260,8 @@ bool KittiParser::loadCamToCamCalibration() {
         // All matrices are written row-major but Eigen is column-major
         // (can swap this, but I guess it's anyway easier to just transpose
         // for these small matrices).
-        camera_calibrations_[index].R = R.transpose();
+        camera_calibrations_[index].T_cam0_cam.getRotation() =
+            Rotation::fromApproximateRotationMatrix(R.transpose());
       }
       continue;
     } else if (header.compare(0, 1, "T") == 0) {
@@ -271,7 +272,7 @@ bool KittiParser::loadCamToCamCalibration() {
       // Parse the translation matrix.
       if (parseVectorOfDoubles(data, &parsed_doubles)) {
         Eigen::Vector3d T(parsed_doubles.data());
-        camera_calibrations_[index].T = T;
+        camera_calibrations_[index].T_cam0_cam.getPosition() = T;
       }
       continue;
     }