minor changes to visualization, jakobi tests
This commit is contained in:
@ -56,6 +56,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
|
||||
is_first_img(true),
|
||||
image_sub(10),
|
||||
nan_flag(false),
|
||||
last_time_bound(0),
|
||||
nh(pnh) {
|
||||
return;
|
||||
}
|
||||
@ -64,6 +65,7 @@ bool MsckfVio::loadParameters() {
|
||||
//Photometry Flag
|
||||
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
|
||||
nh.param<bool>("PrintImages", PRINTIMAGES, false);
|
||||
nh.param<bool>("GroundTruth", GROUNDTRUTH, false);
|
||||
|
||||
// Frame id
|
||||
nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
|
||||
@ -255,6 +257,8 @@ bool MsckfVio::createRosIO() {
|
||||
|
||||
imu_sub = nh.subscribe("imu", 100,
|
||||
&MsckfVio::imuCallback, this);
|
||||
truth_sub = nh.subscribe("ground_truth", 100,
|
||||
&MsckfVio::truthCallback, this);
|
||||
|
||||
cam0_img_sub.subscribe(nh, "cam0_image", 10);
|
||||
cam1_img_sub.subscribe(nh, "cam1_image", 10);
|
||||
@ -263,7 +267,6 @@ bool MsckfVio::createRosIO() {
|
||||
image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub);
|
||||
image_sub.registerCallback(&MsckfVio::imageCallback, this);
|
||||
|
||||
|
||||
mocap_odom_sub = nh.subscribe("mocap_odom", 10,
|
||||
&MsckfVio::mocapOdomCallback, this);
|
||||
mocap_odom_pub = nh.advertise<nav_msgs::Odometry>("gt_odom", 1);
|
||||
@ -292,7 +295,7 @@ bool MsckfVio::initialize() {
|
||||
for (int i = 1; i < 100; ++i) {
|
||||
boost::math::chi_squared chi_squared_dist(i);
|
||||
chi_squared_test_table[i] =
|
||||
boost::math::quantile(chi_squared_dist, 0.05);
|
||||
boost::math::quantile(chi_squared_dist, 0.2);
|
||||
}
|
||||
|
||||
if (!createRosIO()) return false;
|
||||
@ -319,6 +322,10 @@ void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
|
||||
return;
|
||||
}
|
||||
|
||||
void MsckfVio::truthCallback(const geometry_msgs::TransformStampedPtr& msg){
|
||||
truth_msg_buffer.push_back(*msg);
|
||||
}
|
||||
|
||||
void MsckfVio::imageCallback(
|
||||
const sensor_msgs::ImageConstPtr& cam0_img,
|
||||
const sensor_msgs::ImageConstPtr& cam1_img,
|
||||
@ -342,7 +349,10 @@ void MsckfVio::imageCallback(
|
||||
// Propogate the IMU state.
|
||||
// that are received before the image feature_msg.
|
||||
ros::Time start_time = ros::Time::now();
|
||||
batchImuProcessing(feature_msg->header.stamp.toSec());
|
||||
if(!GROUNDTRUTH)
|
||||
batchImuProcessing(feature_msg->header.stamp.toSec());
|
||||
else
|
||||
batchTruthProcessing(feature_msg->header.stamp.toSec());
|
||||
double imu_processing_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
@ -558,6 +568,9 @@ bool MsckfVio::resetCallback(
|
||||
imu_sub = nh.subscribe("imu", 100,
|
||||
&MsckfVio::imuCallback, this);
|
||||
|
||||
truth_sub = nh.subscribe("ground_truth", 100,
|
||||
&MsckfVio::truthCallback, this);
|
||||
|
||||
// feature_sub = nh.subscribe("features", 40,
|
||||
// &MsckfVio::featureCallback, this);
|
||||
|
||||
@ -632,6 +645,71 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
|
||||
return;
|
||||
}
|
||||
|
||||
void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
|
||||
// Counter how many IMU msgs in the buffer are used.
|
||||
int used_truth_msg_cntr = 0;
|
||||
|
||||
for (const auto& truth_msg : truth_msg_buffer) {
|
||||
double truth_time = truth_msg.header.stamp.toSec();
|
||||
if (truth_time < state_server.imu_state.time) {
|
||||
++used_truth_msg_cntr;
|
||||
continue;
|
||||
}
|
||||
if (truth_time > time_bound) break;
|
||||
|
||||
// Convert the msgs.
|
||||
Eigen::Vector3d m_translation;
|
||||
Eigen::Vector4d m_rotation;
|
||||
tf::vectorMsgToEigen(truth_msg.transform.translation, m_translation);
|
||||
|
||||
m_rotation[0] = truth_msg.transform.rotation.x;
|
||||
m_rotation[1] = truth_msg.transform.rotation.y;
|
||||
m_rotation[2] = truth_msg.transform.rotation.z;
|
||||
m_rotation[3] = truth_msg.transform.rotation.w;
|
||||
|
||||
// Execute process model.
|
||||
processTruthtoIMU(truth_time, m_rotation, m_translation);
|
||||
++used_truth_msg_cntr;
|
||||
}
|
||||
last_time_bound = time_bound;
|
||||
|
||||
// Set the state ID for the new IMU state.
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
void MsckfVio::processTruthtoIMU(const double& time,
|
||||
const Vector4d& m_rot,
|
||||
const Vector3d& m_trans){
|
||||
|
||||
IMUState& imu_state = state_server.imu_state;
|
||||
double dtime = time - imu_state.time;
|
||||
|
||||
Vector4d& q = imu_state.orientation;
|
||||
Vector3d& v = imu_state.velocity;
|
||||
Vector3d& p = imu_state.position;
|
||||
|
||||
double dt = time - imu_state.time;
|
||||
|
||||
v = (m_trans-v)/dt;
|
||||
p = m_trans;
|
||||
q = m_rot;
|
||||
|
||||
// Update the state correspondes to null space.
|
||||
imu_state.orientation_null = imu_state.orientation;
|
||||
imu_state.position_null = imu_state.position;
|
||||
imu_state.velocity_null = imu_state.velocity;
|
||||
|
||||
// Update the state info
|
||||
state_server.imu_state.time = time;
|
||||
|
||||
}
|
||||
|
||||
void MsckfVio::batchImuProcessing(const double& time_bound) {
|
||||
// Counter how many IMU msgs in the buffer are used.
|
||||
int used_imu_msg_cntr = 0;
|
||||
@ -1039,17 +1117,22 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
dh_dCpij.block<2, 2>(0, 0) = Eigen::Matrix<double, 2, 2>::Identity();
|
||||
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
|
||||
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
|
||||
|
||||
//orientation takes camera frame to world frame, we wa
|
||||
dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
|
||||
|
||||
//dh / d X_{pl}
|
||||
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose();
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
|
||||
|
||||
//d{}^Gp_P{ij} / \rho_i
|
||||
double rho = feature.anchor_rho;
|
||||
dGpi_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));
|
||||
|
||||
dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho)));
|
||||
dGpi_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
|
||||
* Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
|
||||
feature.anchorPatch_ideal[count].y/(rho),
|
||||
1/(rho)));
|
||||
dGpi_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
|
||||
|
||||
// Intermediate Jakobians
|
||||
@ -1091,6 +1174,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
// set anchor Jakobi
|
||||
// get position of anchor in cam states
|
||||
|
||||
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
|
||||
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
|
||||
H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA;
|
||||
@ -1122,8 +1206,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
for(auto data : photo_r)
|
||||
r[count++] = data;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
if(PRINTIMAGES)
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
|
||||
|
||||
return;
|
||||
}
|
||||
@ -1238,7 +1324,12 @@ void MsckfVio::measurementJacobian(
|
||||
dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
|
||||
|
||||
Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
|
||||
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
|
||||
|
||||
// original jacobi
|
||||
//dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
|
||||
// my version of calculation
|
||||
dpc0_dxc.leftCols(3) = skewSymmetric(R_w_c0 * p_w) - skewSymmetric(R_w_c0 * t_c0_w);
|
||||
//dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
|
||||
dpc0_dxc.rightCols(3) = -R_w_c0;
|
||||
|
||||
Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
|
||||
|
||||
Reference in New Issue
Block a user