raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added 3d visualization and stepping through bag file - minor edits in jakobi

Browse Source
This commit is contained in:
Raphael Maenle
2019-05-09 12:14:12 +02:00
parent 53b26f7613
commit ad2f464716
7 changed files with 277 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
src/msckf_vio.cpp
View File

@ -27,6 +27,10 @@
#include <msckf_vio/math_utils.hpp>
#include <msckf_vio/utils.h>
#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
using namespace std;
using namespace Eigen;
@ -248,9 +252,14 @@ bool MsckfVio::loadParameters() {
}
bool MsckfVio::createRosIO() {
// activating bag playing parameter, for debugging
nh.setParam("/play_bag", true);
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
"feature_point_cloud", 10);
marker_pub = nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);
reset_srv = nh.advertiseService("reset",
&MsckfVio::resetCallback, this);
@ -301,6 +310,19 @@ bool MsckfVio::initialize() {
if (!createRosIO()) return false;
ROS_INFO("Finish creating ROS IO...");
/*
rosbag::Bag bag;
bag.open("/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag", rosbag::bagmode::Read);
for(rosbag::MessageInstance const m: rosbag::View(bag))
{
std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
if (i != NULL)
std::cout << i->data << std::endl;
}
bag.close();
*/
return true;
}
@ -349,10 +371,14 @@ void MsckfVio::imageCallback(
// Propogate the IMU state.
// that are received before the image feature_msg.
ros::Time start_time = ros::Time::now();
if(!GROUNDTRUTH)
batchImuProcessing(feature_msg->header.stamp.toSec());
else
batchImuProcessing(feature_msg->header.stamp.toSec());
// save true state in seperate state vector
if(GROUNDTRUTH)
batchTruthProcessing(feature_msg->header.stamp.toSec());
double imu_processing_time = (
ros::Time::now()-start_time).toSec();
@ -645,6 +671,19 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
return;
}
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 =
}
void MsckfVio::batchTruthProcessing(const double& time_bound) {
// Counter how many IMU msgs in the buffer are used.
@ -652,7 +691,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
for (const auto& truth_msg : truth_msg_buffer) {
double truth_time = truth_msg.header.stamp.toSec();
if (truth_time < state_server.imu_state.time) {
if (truth_time < true_state_server.imu_state.time) {
++used_truth_msg_cntr;
continue;
}
@ -675,7 +714,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
last_time_bound = time_bound;
// Set the state ID for the new IMU state.
state_server.imu_state.id = IMUState::next_id++;
true_state_server.imu_state.id = IMUState::next_id++;
// Remove all used Truth msgs.
truth_msg_buffer.erase(truth_msg_buffer.begin(),
@ -687,7 +726,7 @@ void MsckfVio::processTruthtoIMU(const double& time,
const Vector4d& m_rot,
const Vector3d& m_trans){
IMUState& imu_state = state_server.imu_state;
IMUState& imu_state = true_state_server.imu_state;
double dtime = time - imu_state.time;
Vector4d& q = imu_state.orientation;
@ -706,7 +745,7 @@ void MsckfVio::processTruthtoIMU(const double& time,
imu_state.velocity_null = imu_state.velocity;
// Update the state info
state_server.imu_state.time = time;
true_state_server.imu_state.time = time;
}
@ -905,7 +944,6 @@ void MsckfVio::stateAugmentation(const double& time)
cam_state.orientation_null = cam_state.orientation;
cam_state.position_null = cam_state.position;
// Update the covariance matrix of the state.
// To simplify computation, the matrix J below is the nontrivial block
// in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
@ -945,6 +983,33 @@ void MsckfVio::stateAugmentation(const double& time)
return;
}
void MsckfVio::truePhotometricStateAugmentation(const double& time)
{
const Matrix3d& true_R_i_c = true_state_server.imu_state.R_imu_cam0;
const Vector3d& true_t_c_i = true_state_server.imu_state.t_cam0_imu;
// 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;
Vector3d true_t_c_w = true_state_server.imu_state.position +
R_w_i.transpose()*t_c_i;
true_state_server.cam_states[true_state_server.imu_state.id] =
CAMState(state_server.imu_state.id);
CAMState& true_cam_state = true_state_server.cam_states[
true_state_server.imu_state.id];
true_cam_state.time = time;
true_cam_state.orientation = rotationToQuaternion(true_R_w_c);
true_cam_state.position = t_c_w;
true_cam_state.orientation_null = true_cam_state.orientation;
true_cam_state.position_null = true_cam_state.position;
return;
}
void MsckfVio::PhotometricStateAugmentation(const double& time)
{
@ -1068,9 +1133,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
// 3d feature position in the world frame.
// And its observation with the stereo cameras.
const Vector3d& p_w = feature.position;
//photometric observation
std::vector<double> photo_z;
@ -1080,9 +1142,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
Matrix<double, 3, 1> dGpi_drhoj = Matrix<double, 3, 1>::Zero();
Matrix<double, 3, 6> dGpi_XpAj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
// one line of the NxN Jacobians
Eigen::Matrix<double, 1, 1> H_rhoj;
Eigen::Matrix<double, 1, 6> H_plj;
@ -1099,7 +1165,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
double dx, dy;
for (auto point : feature.anchorPatch_3d)
{
Eigen::Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
//add observation
@ -1118,27 +1184,31 @@ void MsckfVio::PhotometricMeasurementJacobian(
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));
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
//orientation takes camera frame to world frame, we wa
dh_dGpij = dh_dCpij * quaternionToRotation(cam_state.orientation).transpose();
dh_dGpij = dh_dCpij * dCpij_dGpij;
//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();
dCpij_dCGtheta = skewSymmetric(p_c0);
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
//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));
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));
dGpi_XpAj.block<3, 3>(0, 0) = - skewSymmetric(feature.T_anchor_w.linear()
dGpj_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();
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rhoj = dI_dhj * dh_dGpij * dGpi_drhoj; // 1 x 3
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 3
H_plj = dI_dhj * dh_dXplj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpi_XpAj; // 1 x 6
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
H_rho.block<1, 1>(count, 0) = H_rhoj;
H_pl.block<1, 6>(count, 0) = H_plj;
@ -1209,7 +1279,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
if(PRINTIMAGES)
{
feature.MarkerGeneration(marker_pub, state_server.cam_states);
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
}
return;
}
@ -1220,6 +1293,12 @@ void MsckfVio::PhotometricFeatureJacobian(
MatrixXd& H_x, VectorXd& r)
{
// stop playing bagfile if printing images
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", false);
}
const auto& feature = map_server[feature_id];
// Check how many camera states in the provided camera
@ -1278,6 +1357,11 @@ void MsckfVio::PhotometricFeatureJacobian(
H_x = A.transpose() * H_xi;
r = A.transpose() * r_i;
if(PRINTIMAGES)
{
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
}
return;
}