diff --git a/CMakeLists.txt b/CMakeLists.txt index 912cd7c..cc7affb 100644 --- a/CMakeLists.txt +++ b/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} diff --git a/include/msckf_vio/utils.h b/include/msckf_vio/utils.h new file mode 100644 index 0000000..97f0dac --- /dev/null +++ b/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 +#include +#include +#include + +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 diff --git a/src/image_processor.cpp b/src/image_processor.cpp index fcee98b..d4ef1f3 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -16,6 +16,7 @@ #include #include #include +#include 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 cam0_extrinsics(16); - cv::Matx33d R_imu_cam0; - cv::Vec3d t_imu_cam0; - 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]; - + cv::Mat T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu"); + cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3))); + cv::Vec3d t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3)); R_cam0_imu = R_imu_cam0.t(); t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0; - vector cam1_extrinsics(16); - cv::Matx33d R_imu_cam1; - cv::Vec3d t_imu_cam1; - 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]; - + cv::Mat T_imu_cam1 = utils::getTransformCV(nh, "cam1/T_cam_imu"); + cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3))); + cv::Vec3d t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3)); R_cam1_imu = R_imu_cam1.t(); t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index e0b0b01..dc682ce 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -25,6 +25,7 @@ #include #include +#include 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; - vector 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_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu"); Isometry3d T_cam0_imu = T_imu_cam0.inverse(); - Isometry3d T_imu_cam1; - vector 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_imu_cam1 = utils::getTransformEigen(nh, "cam1/T_cam_imu"); Isometry3d T_cam1_imu = T_imu_cam1.inverse(); - Isometry3d T_body_imu; - vector 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]; + Isometry3d T_body_imu = utils::getTransformEigen(nh, "T_imu_body"); //CAMState::T_imu_cam0 = T_cam0_imu.inverse(); state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose(); diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..2af7278 --- /dev/null +++ b/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 +#include + +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(0, 0); + T.linear()(0, 1) = c.at(0, 1); + T.linear()(0, 2) = c.at(0, 2); + T.linear()(1, 0) = c.at(1, 0); + T.linear()(1, 1) = c.at(1, 1); + T.linear()(1, 2) = c.at(1, 2); + T.linear()(2, 0) = c.at(2, 0); + T.linear()(2, 1) = c.at(2, 1); + T.linear()(2, 2) = c.at(2, 2); + T.translation()(0) = c.at(0, 3); + T.translation()(1) = c.at(1, 3); + T.translation()(2) = c.at(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 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(i,j) = static_cast(lines[i][j]); + } + } + } + return T; +} + +} // namespace utils +} // namespace msckf_vio