added 3d visualization and stepping through bag file - minor edits in jakobi
This commit is contained in:
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user