Add common functions for loading calibration with some error checking

From Bernd

CMakeLists.txt

@@ -80,6 +80,7 @@ include_directories(
 # Msckf Vio
 add_library(msckf_vio
   src/msckf_vio.cpp
+  src/utils.cpp
 )
 add_dependencies(msckf_vio
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
@@ -107,6 +108,7 @@ target_link_libraries(msckf_vio_nodelet
 # Image processor
 add_library(image_processor
   src/image_processor.cpp
+  src/utils.cpp
 )
 add_dependencies(image_processor
   ${${PROJECT_NAME}_EXPORTED_TARGETS}

include/msckf_vio/utils.h

@@ -0,0 +1,34 @@
+/*
+ * COPYRIGHT AND PERMISSION NOTICE
+ * Penn Software MSCKF_VIO
+ * Copyright (C) 2017 The Trustees of the University of Pennsylvania
+ * All rights reserved.
+ */
+
+#ifndef MSCKF_VIO_UTILS_H
+#define MSCKF_VIO_UTILS_H
+
+#include <ros/ros.h>
+#include <string>
+#include <opencv2/core/core.hpp>
+#include <Eigen/Geometry>
+
+namespace msckf_vio {
+/*
+ * @brief utilities for msckf_vio
+ */
+namespace utils {
+Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
+                                    const std::string &field);
+
+cv::Mat getTransformCV(const ros::NodeHandle &nh,
+                       const std::string &field);
+
+cv::Mat getVec16Transform(const ros::NodeHandle &nh,
+                          const std::string &field);
+
+cv::Mat getKalibrStyleTransform(const ros::NodeHandle &nh,
+                                const std::string &field);
+}
+}
+#endif

src/image_processor.cpp

@@ -16,6 +16,7 @@
 #include <msckf_vio/CameraMeasurement.h>
 #include <msckf_vio/TrackingInfo.h>
 #include <msckf_vio/image_processor.h>
+#include <msckf_vio/utils.h>
 
 using namespace std;
 using namespace cv;
@@ -86,43 +87,15 @@ bool ImageProcessor::loadParameters() {
   cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
   cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
 
-  vector<double> cam0_extrinsics(16);
+  cv::Mat     T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
-  cv::Matx33d R_imu_cam0;
+  cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
-  cv::Vec3d t_imu_cam0;
+  cv::Vec3d   t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
-  nh.getParam("cam0/T_cam_imu", cam0_extrinsics);
-  R_imu_cam0(0, 0) = cam0_extrinsics[0];
-  R_imu_cam0(0, 1) = cam0_extrinsics[1];
-  R_imu_cam0(0, 2) = cam0_extrinsics[2];
-  R_imu_cam0(1, 0) = cam0_extrinsics[4];
-  R_imu_cam0(1, 1) = cam0_extrinsics[5];
-  R_imu_cam0(1, 2) = cam0_extrinsics[6];
-  R_imu_cam0(2, 0) = cam0_extrinsics[8];
-  R_imu_cam0(2, 1) = cam0_extrinsics[9];
-  R_imu_cam0(2, 2) = cam0_extrinsics[10];
-  t_imu_cam0(0) = cam0_extrinsics[3];
-  t_imu_cam0(1) = cam0_extrinsics[7];
-  t_imu_cam0(2) = cam0_extrinsics[11];
-
   R_cam0_imu = R_imu_cam0.t();
   t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
 
-  vector<double> cam1_extrinsics(16);
+  cv::Mat     T_imu_cam1 = utils::getTransformCV(nh, "cam1/T_cam_imu");
-  cv::Matx33d R_imu_cam1;
+  cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
-  cv::Vec3d t_imu_cam1;
+  cv::Vec3d   t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
-  nh.getParam("cam1/T_cam_imu", cam1_extrinsics);
-  R_imu_cam1(0, 0) = cam1_extrinsics[0];
-  R_imu_cam1(0, 1) = cam1_extrinsics[1];
-  R_imu_cam1(0, 2) = cam1_extrinsics[2];
-  R_imu_cam1(1, 0) = cam1_extrinsics[4];
-  R_imu_cam1(1, 1) = cam1_extrinsics[5];
-  R_imu_cam1(1, 2) = cam1_extrinsics[6];
-  R_imu_cam1(2, 0) = cam1_extrinsics[8];
-  R_imu_cam1(2, 1) = cam1_extrinsics[9];
-  R_imu_cam1(2, 2) = cam1_extrinsics[10];
-  t_imu_cam1(0) = cam1_extrinsics[3];
-  t_imu_cam1(1) = cam1_extrinsics[7];
-  t_imu_cam1(2) = cam1_extrinsics[11];
-
   R_cam1_imu = R_imu_cam1.t();
   t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;
 

src/msckf_vio.cpp

@@ -25,6 +25,7 @@
 
 #include <msckf_vio/msckf_vio.h>
 #include <msckf_vio/math_utils.hpp>
+#include <msckf_vio/utils.h>
 
 using namespace std;
 using namespace Eigen;
@@ -128,55 +129,13 @@ bool MsckfVio::loadParameters() {
     state_server.state_cov(i, i) = extrinsic_translation_cov;
 
   // Transformation offsets between the frames involved.
-  Isometry3d T_imu_cam0;
+  Isometry3d T_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu");
-  vector<double> cam0_extrinsics(16);
-  nh.getParam("cam0/T_cam_imu", cam0_extrinsics);
-  T_imu_cam0.linear()(0, 0) = cam0_extrinsics[0];
-  T_imu_cam0.linear()(0, 1) = cam0_extrinsics[1];
-  T_imu_cam0.linear()(0, 2) = cam0_extrinsics[2];
-  T_imu_cam0.linear()(1, 0) = cam0_extrinsics[4];
-  T_imu_cam0.linear()(1, 1) = cam0_extrinsics[5];
-  T_imu_cam0.linear()(1, 2) = cam0_extrinsics[6];
-  T_imu_cam0.linear()(2, 0) = cam0_extrinsics[8];
-  T_imu_cam0.linear()(2, 1) = cam0_extrinsics[9];
-  T_imu_cam0.linear()(2, 2) = cam0_extrinsics[10];
-  T_imu_cam0.translation()(0) = cam0_extrinsics[3];
-  T_imu_cam0.translation()(1) = cam0_extrinsics[7];
-  T_imu_cam0.translation()(2) = cam0_extrinsics[11];
   Isometry3d T_cam0_imu = T_imu_cam0.inverse();
 
-  Isometry3d T_imu_cam1;
+  Isometry3d T_imu_cam1 = utils::getTransformEigen(nh, "cam1/T_cam_imu");
-  vector<double> cam1_extrinsics(16);
-  nh.getParam("cam1/T_cam_imu", cam1_extrinsics);
-  T_imu_cam1.linear()(0, 0) = cam1_extrinsics[0];
-  T_imu_cam1.linear()(0, 1) = cam1_extrinsics[1];
-  T_imu_cam1.linear()(0, 2) = cam1_extrinsics[2];
-  T_imu_cam1.linear()(1, 0) = cam1_extrinsics[4];
-  T_imu_cam1.linear()(1, 1) = cam1_extrinsics[5];
-  T_imu_cam1.linear()(1, 2) = cam1_extrinsics[6];
-  T_imu_cam1.linear()(2, 0) = cam1_extrinsics[8];
-  T_imu_cam1.linear()(2, 1) = cam1_extrinsics[9];
-  T_imu_cam1.linear()(2, 2) = cam1_extrinsics[10];
-  T_imu_cam1.translation()(0) = cam1_extrinsics[3];
-  T_imu_cam1.translation()(1) = cam1_extrinsics[7];
-  T_imu_cam1.translation()(2) = cam1_extrinsics[11];
   Isometry3d T_cam1_imu = T_imu_cam1.inverse();
 
-  Isometry3d T_body_imu;
+  Isometry3d T_body_imu = utils::getTransformEigen(nh, "T_imu_body");
-  vector<double> imu_extrinsics(16);
-  nh.getParam("T_imu_body", imu_extrinsics);
-  T_body_imu.linear()(0, 0) = imu_extrinsics[0];
-  T_body_imu.linear()(0, 1) = imu_extrinsics[1];
-  T_body_imu.linear()(0, 2) = imu_extrinsics[2];
-  T_body_imu.linear()(1, 0) = imu_extrinsics[4];
-  T_body_imu.linear()(1, 1) = imu_extrinsics[5];
-  T_body_imu.linear()(1, 2) = imu_extrinsics[6];
-  T_body_imu.linear()(2, 0) = imu_extrinsics[8];
-  T_body_imu.linear()(2, 1) = imu_extrinsics[9];
-  T_body_imu.linear()(2, 2) = imu_extrinsics[10];
-  T_body_imu.translation()(0) = imu_extrinsics[3];
-  T_body_imu.translation()(1) = imu_extrinsics[7];
-  T_body_imu.translation()(2) = imu_extrinsics[11];
 
   //CAMState::T_imu_cam0 = T_cam0_imu.inverse();
   state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();

src/utils.cpp

@@ -0,0 +1,92 @@
+/*
+ * COPYRIGHT AND PERMISSION NOTICE
+ * Penn Software MSCKF_VIO
+ * Copyright (C) 2017 The Trustees of the University of Pennsylvania
+ * All rights reserved.
+ */
+
+#include <msckf_vio/utils.h>
+#include <vector>
+
+namespace msckf_vio {
+namespace utils {
+
+Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
+                                    const std::string &field) {
+  Eigen::Isometry3d T;
+  cv::Mat c = getTransformCV(nh, field);
+
+  T.linear()(0, 0)   = c.at<double>(0, 0);
+  T.linear()(0, 1)   = c.at<double>(0, 1);
+  T.linear()(0, 2)   = c.at<double>(0, 2);
+  T.linear()(1, 0)   = c.at<double>(1, 0);
+  T.linear()(1, 1)   = c.at<double>(1, 1);
+  T.linear()(1, 2)   = c.at<double>(1, 2);
+  T.linear()(2, 0)   = c.at<double>(2, 0);
+  T.linear()(2, 1)   = c.at<double>(2, 1);
+  T.linear()(2, 2)   = c.at<double>(2, 2);
+  T.translation()(0) = c.at<double>(0, 3);
+  T.translation()(1) = c.at<double>(1, 3);
+  T.translation()(2) = c.at<double>(2, 3);
+  return T;
+}
+
+cv::Mat getTransformCV(const ros::NodeHandle &nh,
+                       const std::string &field) {
+  cv::Mat T;
+  try {
+    // first try reading kalibr format
+    T = getKalibrStyleTransform(nh, field);
+  } catch (std::runtime_error &e) {
+    // maybe it's the old style format?
+    ROS_DEBUG_STREAM("cannot read transform " << field
+                     << " in kalibr format, trying old one!");
+    try {
+      T = getVec16Transform(nh, field);
+    } catch (std::runtime_error &e) {
+      std::string msg = "cannot read transform " + field + " error: " + e.what();
+      ROS_ERROR_STREAM(msg);
+      throw std::runtime_error(msg);
+    }
+  }
+  return T;
+}
+
+cv::Mat getVec16Transform(const ros::NodeHandle &nh,
+                          const std::string &field) {
+  std::vector<double> v;
+  nh.getParam(field, v);
+  if (v.size() != 16) {
+    throw std::runtime_error("invalid vec16!");
+  }
+  cv::Mat T = cv::Mat(v).clone().reshape(1, 4); // one channel 4 rows
+  return T;
+}
+
+cv::Mat getKalibrStyleTransform(const ros::NodeHandle &nh,
+                                const std::string &field) {
+  cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1);
+  XmlRpc::XmlRpcValue lines;
+  if (!nh.getParam(field, lines)) {
+    throw (std::runtime_error("cannot find transform " + field));
+  }
+  if (lines.size() != 4 || lines.getType() != XmlRpc::XmlRpcValue::TypeArray) {
+    throw (std::runtime_error("invalid transform " + field));
+  }
+  for (int i = 0; i < lines.size(); i++) {
+    if (lines.size() != 4 || lines.getType() != XmlRpc::XmlRpcValue::TypeArray) {
+      throw (std::runtime_error("bad line for transform " + field));
+    }
+    for (int j = 0; j < lines[i].size(); j++) {
+      if (lines[i][j].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
+        throw (std::runtime_error("bad value for transform " + field));
+      } else {
+        T.at<double>(i,j) = static_cast<double>(lines[i][j]);
+      }
+    }
+  }
+  return T;
+}
+
+} // namespace utils
+} // namespace msckf_vio