From 44de215518d7caea2d1640467ba6373ac4b8c388 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 10 May 2019 17:19:29 +0200 Subject: [PATCH] lots of additional debugging tools implemented to check parts of the algorithm. still no good --- include/msckf_vio/math_utils.hpp | 31 ++++- include/msckf_vio/msckf_vio.h | 6 + launch/msckf_vio_debug_tum.launch | 4 +- launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 197 ++++++++++++++++++++++++++---- 5 files changed, 211 insertions(+), 29 deletions(-) diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp index 29fb540..d6cb57a 100644 --- a/include/msckf_vio/math_utils.hpp +++ b/include/msckf_vio/math_utils.hpp @@ -54,10 +54,39 @@ inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q) p(1) = -q(1); p(2) = -q(2); p(3) = q(3); - + quaternionNormalize(p); return p; } +/* + * @brief converts a vector4 to a vector3, dropping (3) + * this is typically used to get the vector part of a quaternion + for eq small angle approximation +*/ +inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q) +{ + Eigen::Vector3d p; + p(0) = q(0); + p(1) = q(1); + p(2) = q(2); + return p; +} + +/* + * @brief Perform q1 * q2 + */ + +inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q) +{ + Eigen::Vector4d p; + p(0) = q.x(); + p(1) = q.y(); + p(2) = q.z(); + p(3) = q.w(); + return p; +} + + /* * @brief Perform q1 * q2 */ diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 27b0ef5..bcf5ee2 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -160,11 +160,14 @@ class MsckfVio { const CameraMeasurementConstPtr& feature_msg); + void calcErrorState(); + // Debug related Functions // Propagate the true state void batchTruthProcessing( const double& time_bound); + void processTruthtoIMU(const double& time, const Eigen::Vector4d& m_rot, const Eigen::Vector3d& m_trans); @@ -339,6 +342,9 @@ class MsckfVio { ros::Publisher mocap_odom_pub; geometry_msgs::TransformStamped raw_mocap_odom_msg; Eigen::Isometry3d mocap_initial_frame; + + Eigen::Vector4d mocap_initial_orientation; + Eigen::Vector3d mocap_initial_position; }; typedef MsckfVio::Ptr MsckfVioPtr; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index 7b3b45f..c4e12e7 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -22,8 +22,8 @@ - - + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..38f93a2 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,7 +22,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 8289cb3..2473996 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -203,6 +203,9 @@ bool MsckfVio::loadParameters() { state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose(); state_server.imu_state.t_cam0_imu = T_cam0_imu.translation(); + true_state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose(); + true_state_server.imu_state.t_cam0_imu = T_cam0_imu.translation(); + CAMState::T_cam0_cam1 = utils::getTransformEigen(nh, "cam1/T_cn_cnm1"); IMUState::T_imu_body = @@ -345,6 +348,54 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){ } void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){ + static bool first_truth_odom_msg = true; + + // errorstate + /*if(not ErrorState) + return; + */ + + // If this is the first mocap odometry messsage, set + // the initial frame. + if (first_truth_odom_msg) { + Quaterniond orientation; + Vector3d translation; + tf::vectorMsgToEigen( + msg->transform.translation, translation); + tf::quaternionMsgToEigen( + msg->transform.rotation, orientation); + + mocap_initial_orientation = QtoV(orientation); + mocap_initial_position = translation; + + first_truth_odom_msg = false; + } + + // Transform the ground truth. + Quaterniond orientation; + Vector3d translation; + //tf::vectorMsgToEigen( + // msg->transform.translation, translation); + //tf::quaternionMsgToEigen( + // msg->transform.rotation, orientation); + tf::vectorMsgToEigen( + msg->transform.translation, translation); + tf::quaternionMsgToEigen( + msg->transform.rotation, orientation); + + Eigen::Vector4d q = quaternionMultiplication(quaternionConjugate(mocap_initial_orientation), QtoV(orientation)); + + translation -= mocap_initial_position; + + msg->transform.rotation.x = q(0); + msg->transform.rotation.y = q(1); + msg->transform.rotation.z = q(2); + msg->transform.rotation.w = q(3); + + msg->transform.translation.x = translation(0); + msg->transform.translation.y = translation(1); + msg->transform.translation.z = translation(2); + truth_msg_buffer.push_back(*msg); } @@ -376,16 +427,32 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); // save true state in seperate state vector - if(GROUNDTRUTH) + + //if(ErrState) + //{ batchTruthProcessing(feature_msg->header.stamp.toSec()); + + if(GROUNDTRUTH) + { + state_server.imu_state.position = true_state_server.imu_state.position; + state_server.imu_state.orientation = true_state_server.imu_state.orientation; + state_server.imu_state.position_null = true_state_server.imu_state.position_null; + state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; + state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; + state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; + } + //} double imu_processing_time = ( ros::Time::now()-start_time).toSec(); // Augment the state vector. start_time = ros::Time::now(); if(PHOTOMETRIC) + { + truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); + } else stateAugmentation(feature_msg->header.stamp.toSec()); double state_augmentation_time = ( @@ -673,15 +740,47 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { void MsckfVio::calcErrorState() { - // true_state_server - state_server = err_state_server - StateServer errState; - errState.imu_state.id = state_server.imu_state.id; - errState.imu_state.time = state-server.imu_state.time; - errState.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaterionConjugate(state_server.imu_state.orientation)); - errState.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position; - errState.imu_state.velocity = + // imu error + err_state_server.imu_state.id = state_server.imu_state.id; + err_state_server.imu_state.time = state_server.imu_state.time; + err_state_server.imu_state.orientation = quaternionMultiplication(true_state_server.imu_state.orientation, quaternionConjugate(state_server.imu_state.orientation)); + + // convert to small angle approximation + err_state_server.imu_state.orientation *= 2; + err_state_server.imu_state.orientation(3) = 0; + + err_state_server.imu_state.position = true_state_server.imu_state.position - state_server.imu_state.position; + err_state_server.imu_state.velocity = true_state_server.imu_state.velocity - state_server.imu_state.velocity; + + err_state_server.imu_state.gyro_bias = true_state_server.imu_state.gyro_bias - true_state_server.imu_state.gyro_bias; + err_state_server.imu_state.gyro_bias = true_state_server.imu_state.acc_bias - true_state_server.imu_state.acc_bias; + + err_state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0 - true_state_server.imu_state.R_imu_cam0; + err_state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu - true_state_server.imu_state.t_cam0_imu; + + err_state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null - true_state_server.imu_state.orientation_null; + err_state_server.imu_state.position_null = true_state_server.imu_state.position_null - true_state_server.imu_state.position_null; + err_state_server.imu_state.velocity_null = true_state_server.imu_state.velocity_null - true_state_server.imu_state.velocity_null; + + auto cam_state_iter = state_server.cam_states.begin(); + auto true_cam_state_iter = true_state_server.cam_states.begin(); + auto err_cam_state_iter = err_state_server.cam_states.begin(); + + for (int i = 0; i < state_server.cam_states.size(); ++i, ++cam_state_iter, ++err_cam_state_iter, ++true_cam_state_iter) + { + // calculate error in camera rotation + Eigen::Vector4d q = cam_state_iter->second.orientation; + Eigen::Vector4d p = quaternionConjugate(true_cam_state_iter->second.orientation); + err_cam_state_iter->second.orientation = quaternionMultiplication(p, q); + // small angle approximation + err_cam_state_iter->second.orientation *= 2; + err_cam_state_iter->second.orientation(3) = 0; + + // calculate error of state position + err_cam_state_iter->second.position = true_cam_state_iter->second.position - cam_state_iter->second.position; + } } void MsckfVio::batchTruthProcessing(const double& time_bound) { @@ -706,7 +805,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { m_rotation[1] = truth_msg.transform.rotation.y; m_rotation[2] = truth_msg.transform.rotation.z; m_rotation[3] = truth_msg.transform.rotation.w; - + quaternionNormalize(m_rotation); // Execute process model. processTruthtoIMU(truth_time, m_rotation, m_translation); ++used_truth_msg_cntr; @@ -714,8 +813,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { last_time_bound = time_bound; // Set the state ID for the new IMU state. - true_state_server.imu_state.id = IMUState::next_id++; - + true_state_server.imu_state.id = IMUState::next_id; + err_state_server.imu_state.id = IMUState::next_id; // Remove all used Truth msgs. truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); @@ -960,7 +1059,6 @@ void MsckfVio::stateAugmentation(const double& time) size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); - // add 7 for camera state + irradiance bias eta = b_l state_server.state_cov.conservativeResize(old_rows+6, old_cols+6); // Rename some matrix blocks for convenience. @@ -991,18 +1089,26 @@ void MsckfVio::truePhotometricStateAugmentation(const double& time) // Add a new camera state to the state server. Matrix3d true_R_w_i = quaternionToRotation( true_state_server.imu_state.orientation); - Matrix3d true_R_w_c = R_i_c * R_w_i; + Matrix3d true_R_w_c = true_R_i_c * true_R_w_i; Vector3d true_t_c_w = true_state_server.imu_state.position + - R_w_i.transpose()*t_c_i; + true_R_w_i.transpose()*true_t_c_i; true_state_server.cam_states[true_state_server.imu_state.id] = - CAMState(state_server.imu_state.id); + CAMState(true_state_server.imu_state.id); CAMState& true_cam_state = true_state_server.cam_states[ true_state_server.imu_state.id]; + // manage error state size + err_state_server.cam_states[err_state_server.imu_state.id] = + CAMState(err_state_server.imu_state.id); + CAMState& err_cam_state = err_state_server.cam_states[ + err_state_server.imu_state.id]; + + err_cam_state.time = time; + true_cam_state.time = time; true_cam_state.orientation = rotationToQuaternion(true_R_w_c); - true_cam_state.position = t_c_w; + true_cam_state.position = true_t_c_w; true_cam_state.orientation_null = true_cam_state.orientation; true_cam_state.position_null = true_cam_state.position; @@ -1163,6 +1269,7 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; + for (auto point : feature.anchorPatch_3d) { Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); @@ -1197,6 +1304,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; + // Isometry T_anchor_w takes a vector in anchor frame to world frame dGpj_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); dGpj_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear() @@ -1216,7 +1324,6 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } - // calculate residual //observation @@ -1299,7 +1406,26 @@ void MsckfVio::PhotometricFeatureJacobian( std::cout << "stopped playpack" << std::endl; nh.setParam("/play_bag", false); } + + + // Errstate + calcErrorState(); + /* + std::cout << "--- error state: ---\n " << std::endl; + std::cout << "imu orientation:\n " << err_state_server.imu_state.orientation << std::endl; + std::cout << "imu position\n" << err_state_server.imu_state.position << std::endl; + + int count = 0; + for(auto cam_state : err_state_server.cam_states) + { + std::cout << " - cam " << count++ << " - \n" << std::endl; + std::cout << "orientation: " << cam_state.second.orientation(0) << cam_state.second.orientation(1) << " " << cam_state.second.orientation(2) << " " << std::endl; + std::cout << "position:" << cam_state.second.position(0) << " " << cam_state.second.position(1) << " " << cam_state.second.position(2) << std::endl; + } + */ + const auto& feature = map_server[feature_id]; + // Check how many camera states in the provided camera // id camera has actually seen this feature. @@ -1344,18 +1470,24 @@ void MsckfVio::PhotometricFeatureJacobian( // get Nullspace JacobiSVD svd_helper(H_yi, ComputeFullU | ComputeThinV); + int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] > 1e-5) + if(singularValues[i] > 1e-9) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); - MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size-sv_size); + int null_space_size = svd_helper.matrixU().cols() - sv_size; //TEST used instead of svd_helper.singularValues().size(); + MatrixXd A = svd_helper.matrixU().rightCols(null_space_size); H_x = A.transpose() * H_xi; r = A.transpose() * r_i; + + ofstream myfile; + myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); + myfile << "nulls:\n" << A.transpose() * H_yi <width = feature_msg_ptr->points.size(); feature_pub.publish(feature_msg_ptr); - return; }