From ad2f4647163152ac19b5a964370ab8bb0ed6ea76 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Thu, 9 May 2019 12:14:12 +0200 Subject: [PATCH] added 3d visualization and stepping through bag file - minor edits in jakobi --- CMakeLists.txt | 1 + include/msckf_vio/feature.hpp | 146 +++++++++++++++++++++++++++--- include/msckf_vio/math_utils.hpp | 15 +++ include/msckf_vio/msckf_vio.h | 13 ++- launch/msckf_vio_debug_tum.launch | 6 ++ package.xml | 1 + src/msckf_vio.cpp | 130 +++++++++++++++++++++----- 7 files changed, 277 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f5770a..99992f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_conversions pcl_ros std_srvs + visualization_msgs ) ## System dependencies are found with CMake's conventions diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index bf71e2e..8a6cc27 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -16,12 +16,17 @@ #include #include +#include +#include +#include + #include "image_handler.h" #include "math_utils.hpp" #include "imu_state.h" #include "cam_state.h" + namespace msckf_vio { /* @@ -168,6 +173,10 @@ struct Feature { std::vector& anchorPatch_estimate, IlluminationParameter& estimatedIllumination) const; +bool MarkerGeneration( + ros::Publisher& marker_pub, + const CamStateServer& cam_states) const; + bool VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -207,7 +216,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, std::vector anchorPatch_ideal; std::vector anchorPatch_real; - // Position of NxN Patch in 3D space + // Position of NxN Patch in 3D space in anchor camera frame std::vector anchorPatch_3d; // Anchor Isometry @@ -406,6 +415,118 @@ bool Feature::estimate_FrameIrradiance( } } +// generates markers for every camera position/observation +// and estimated feature/path position +bool Feature::MarkerGeneration( + ros::Publisher& marker_pub, + const CamStateServer& cam_states) const +{ + visualization_msgs::MarkerArray ma; + + // add all camera states used for estimation + int count = 0; + + for(auto observation : observations) + { + visualization_msgs::Marker marker; + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "cameras"; + marker.id = count++; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.position.x = cam_states.find(observation.first)->second.position(0); + marker.pose.position.y = cam_states.find(observation.first)->second.position(1); + marker.pose.position.z = cam_states.find(observation.first)->second.position(2); + + // rotate form x to z axis + Eigen::Vector4d q = quaternionMultiplication(Eigen::Vector4d(0, -0.707, 0, 0.707), cam_states.find(observation.first)->second.orientation); + + marker.pose.orientation.x = q(0); + marker.pose.orientation.y = q(1); + marker.pose.orientation.z = q(2); + marker.pose.orientation.w = q(3); + + marker.scale.x = 0.15; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + + if(count == 1) + { + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + } + else + { + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + } + marker.color.a = 1.0; + + marker.lifetime = ros::Duration(0); + + ma.markers.push_back(marker); + } + + // 'delete' any existing cameras (make invisible) + for(int i = count; i < 20; i++) + { + visualization_msgs::Marker marker; + + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "cameras"; + marker.id = i; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.orientation.w = 1; + + marker.color.a = 0.0; + + marker.lifetime = ros::Duration(1); + + ma.markers.push_back(marker); + } + + //generate feature patch points position + visualization_msgs::Marker marker; + + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + + marker.ns = "patch"; + marker.id = 0; + marker.type = visualization_msgs::Marker::POINTS; + marker.action = visualization_msgs::Marker::ADD; + + marker.pose.orientation.w = 1; + + marker.scale.x = 0.02; + marker.scale.y = 0.02; + + marker.color.r = 1.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0; + + for(auto point : anchorPatch_3d) + { + geometry_msgs::Point p; + p.x = point(0); + p.y = point(1); + p.z = point(2); + marker.points.push_back(p); + } + ma.markers.push_back(marker); + + marker_pub.publish(ma); +} bool Feature::VisualizePatch( const CAMState& cam_state, @@ -489,12 +610,15 @@ bool Feature::VisualizePatch( cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), CV_FILLED); - + cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); + +/* // visualize position of used observations and resulting feature position cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255)); cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale); + // draw world zero cv::line(positionFrame, cv::Point(20,20), @@ -510,20 +634,20 @@ bool Feature::VisualizePatch( // for every observation, get cam state for(auto obs : observations) { - cam_state.find(obs->first); cv::line(positionFrame, cv::Point(20,20), cv::Point(30,20), cv::Scalar(255,0,0), CV_FILLED); } + // draw, x y position and arrow with direction - write z next to it cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale); - cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu); - cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); + cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu); +*/ // write feature position std::stringstream pos_s; pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1); @@ -534,11 +658,11 @@ bool Feature::VisualizePatch( //save image std::stringstream loc; - loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; - cv::imwrite(loc.str(), cam0.featureVisu); + // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg"; + //cv::imwrite(loc.str(), cam0.featureVisu); - //cv::imshow("patch", cam0.featureVisu); - //cvWaitKey(1); + cv::imshow("patch", cam0.featureVisu); + cvWaitKey(0); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const @@ -586,7 +710,7 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCa // save resulting NxN positions for this feature Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho); - Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); + Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation(); return PositionInWorld; //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]); } @@ -653,7 +777,7 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) for(auto point : anchorPatch_real) anchorPatch.push_back(PixelIrradiance(point, anchorImage)); - // project patch pixel to 3D space + // project patch pixel to 3D space in camera coordinate system for(auto point : und_v) { anchorPatch_ideal.push_back(point); diff --git a/include/msckf_vio/math_utils.hpp b/include/msckf_vio/math_utils.hpp index 3000d4c..29fb540 100644 --- a/include/msckf_vio/math_utils.hpp +++ b/include/msckf_vio/math_utils.hpp @@ -43,6 +43,21 @@ inline void quaternionNormalize(Eigen::Vector4d& q) { return; } +/* + * @brief invert rotation of passed quaternion through conjugation + * and return conjugation + */ +inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q) +{ + Eigen::Vector4d p; + p(0) = -q(0); + p(1) = -q(1); + p(2) = -q(2); + p(3) = q(3); + + return p; +} + /* * @brief Perform q1 * q2 */ diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 606d037..27b0ef5 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -181,6 +181,9 @@ class MsckfVio { const Eigen::Vector3d& gyro, const Eigen::Vector3d& acc); + // groundtruth state augmentation + void truePhotometricStateAugmentation(const double& time); + // Measurement update void stateAugmentation(const double& time); void PhotometricStateAugmentation(const double& time); @@ -230,7 +233,7 @@ class MsckfVio { bool GROUNDTRUTH; bool nan_flag; - + bool play; double last_time_bound; // Patch size for Photometry @@ -241,6 +244,13 @@ class MsckfVio { // State vector StateServer state_server; + + // Ground truth state vector + StateServer true_state_server; + + // error state based on ground truth + StateServer err_state_server; + // Maximum number of camera states int max_cam_state_size; @@ -297,6 +307,7 @@ class MsckfVio { ros::Subscriber imu_sub; ros::Subscriber truth_sub; ros::Publisher odom_pub; + ros::Publisher marker_pub; ros::Publisher feature_pub; tf::TransformBroadcaster tf_pub; ros::ServiceServer reset_srv; diff --git a/launch/msckf_vio_debug_tum.launch b/launch/msckf_vio_debug_tum.launch index e80c5ef..7b3b45f 100644 --- a/launch/msckf_vio_debug_tum.launch +++ b/launch/msckf_vio_debug_tum.launch @@ -21,6 +21,12 @@ + + + + + + diff --git a/package.xml b/package.xml index 86a6e20..cf1b6f6 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,7 @@ nav_msgs sensor_msgs geometry_msgs + visualization_msgs eigen_conversions tf_conversions random_numbers diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 8123150..8289cb3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -27,6 +27,10 @@ #include #include +#include +#include +#include + 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("odom", 10); feature_pub = nh.advertise( "feature_point_cloud", 10); + marker_pub = nh.advertise("/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(); + 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 photo_z; @@ -1080,9 +1142,13 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix dh_dCpij = Matrix::Zero(); Matrix dh_dGpij = Matrix::Zero(); Matrix dh_dXplj = Matrix::Zero(); - Matrix dGpi_drhoj = Matrix::Zero(); - Matrix dGpi_XpAj = Matrix::Zero(); - + Matrix dGpj_drhoj = Matrix::Zero(); + Matrix dGpj_XpAj = Matrix::Zero(); + + Matrix dCpij_dGpij = Matrix::Zero(); + Matrix dCpij_dCGtheta = Matrix::Zero(); + Matrix dCpij_dGpC = Matrix::Zero(); + // one line of the NxN Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix 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::Identity(); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::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; }