lots of additional debugging tools implemented to check parts of the algorithm. still no good
This commit is contained in:
parent
ad2f464716
commit
44de215518
@ -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
|
||||
*/
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -22,8 +22,8 @@
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="7"/>
|
||||
|
||||
|
||||
@ -22,7 +22,7 @@
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
<param name="GroundTruth" value="true"/>
|
||||
|
||||
<param name="patch_size_n" value="7"/>
|
||||
<!-- Calibration parameters -->
|
||||
|
||||
@ -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,8 +1406,27 @@ 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.
|
||||
vector<StateIDType> valid_cam_state_ids(0);
|
||||
@ -1344,19 +1470,25 @@ 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)
|
||||
{
|
||||
std::cout << "resume playback" << std::endl;
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user