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

image reprojection visualization in images

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-19 13:11:19 +02:00
parent 1fa2518215
commit 6f16f1b566
4 changed files with 99 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
src/msckf_vio.cpp
View File

@@ -220,6 +220,12 @@ bool MsckfVio::loadParameters() {
cout << T_imu_cam0.linear() << endl;
cout << T_imu_cam0.translation().transpose() << endl;
cout << "OpenCV version : " << CV_VERSION << endl;
cout << "Major version : " << CV_MAJOR_VERSION << endl;
cout << "Minor version : " << CV_MINOR_VERSION << endl;
cout << "Subminor version : " << CV_SUBMINOR_VERSION << endl;
ROS_INFO("max camera state #: %d", max_cam_state_size);
ROS_INFO("===========================================");
return true;
@@ -392,37 +398,37 @@ void MsckfVio::imageCallback(
}
void MsckfVio::manageMovingWindow(
const sensor_msgs::ImageConstPtr& cam0_img,
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg) {
const sensor_msgs::ImageConstPtr& cam0_img,
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg) {
//save exposure Time into moving window
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam0_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 100;
//save exposure Time into moving window
cam0.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
cam1.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms < 1)
cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 1;
if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 100;
if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms > 100)
cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 100;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
// save image information into moving window
cam0_moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
cam1_moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
// save image information into moving window
cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
while(cam0_moving_window.size() > 100)
while(cam0.moving_window.size() > 100)
{
cam1_moving_window.erase(cam1_moving_window.begin());
cam0_moving_window.erase(cam0_moving_window.begin());
cam1.moving_window.erase(cam1.moving_window.begin());
cam0.moving_window.erase(cam0.moving_window.begin());
}
}
@@ -916,7 +922,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation
std::vector<float> photo_z;
feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
feature.FrameIrradiance(cam_state, cam_state_id, cam0, photo_z);
// Convert the feature position from the world frame to
// the cam0 and cam1 frame.
@@ -975,7 +981,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//estimate photometric measurement
std::vector<float> estimate_photo_z;
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z);
std::vector<float> photo_r;
//calculate photom. residual
@@ -1017,6 +1023,8 @@ void MsckfVio::PhotometricFeatureJacobian(
int stack_cntr = 0;
printf("_____FEATURE:_____\n");
cam0.featureVisu.release();
for (const auto& cam_id : valid_cam_state_ids) {
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
@@ -1034,7 +1042,17 @@ void MsckfVio::PhotometricFeatureJacobian(
r_j.segment<4>(stack_cntr) = r_i;
stack_cntr += 4;
}
if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
imshow("feature", cam0.featureVisu);
cvWaitKey(1);
if((ros::Time::now() - image_save_time).toSec() > 1)
{
std::stringstream ss;
ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
imwrite(ss.str(), cam0.featureVisu);
image_save_time = ros::Time::now();
}
// Project the residual and Jacobians onto the nullspace
// of H_fj.
JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
@@ -1336,7 +1354,7 @@ void MsckfVio::removeLostFeatures() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
if(!feature.initializeAnchor(cam0))
{
invalid_feature_ids.push_back(feature.id);
continue;
@@ -1491,7 +1509,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
if(!feature.is_anchored)
{
if(!feature.initializeAnchor(cam0_moving_window, cam0))
if(!feature.initializeAnchor(cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
@@ -1573,8 +1591,8 @@ 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);
cam0.moving_window.erase(cam_id);
cam1.moving_window.erase(cam_id);
}
return;