lots of additional debugging tools implemented to check parts of the algorithm. still no good

2019-05-10 17:19:29 +02:00
parent ad2f464716
commit 44de215518
5 changed files with 211 additions and 29 deletions
--- 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<MatrixXd> 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 <<endl;
+  myfile.close();
+  cout << "---------- LOGGED -------- " << endl;

  if(PRINTIMAGES)
  {
@@ -1495,6 +1627,13 @@ void MsckfVio::featureJacobian(
   H_x = A.transpose() * H_xj;
   r = A.transpose() * r_j;

+  ofstream myfile;
+  myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+  myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
+  myfile.close();
+  cout << "---------- LOGGED -------- " << endl;
+  nh.setParam("/play_bag", false);
+
  return;
 }

@@ -1926,9 +2065,18 @@ void MsckfVio::pruneCamStateBuffer() {
    }

    // Remove this camera state in the state vector.
-    state_server.cam_states.erase(cam_id);
-    cam0.moving_window.erase(cam_id);
-    cam1.moving_window.erase(cam_id);
+
+
+    for (const auto& cam_id : rm_cam_state_ids) {
+
+      state_server.cam_states.erase(cam_id);
+      cam0.moving_window.erase(cam_id);
+      cam1.moving_window.erase(cam_id);
+      
+      // Remove this camera state in the true state vector.
+      true_state_server.cam_states.erase(cam_id);
+      err_state_server.cam_states.erase(cam_id);
+    }
  }

  return;
@@ -2084,7 +2232,6 @@ void MsckfVio::publish(const ros::Time& time) {
  feature_msg_ptr->width = feature_msg_ptr->points.size();

  feature_pub.publish(feature_msg_ptr);
-
  return;
 }