diff --git a/config/camchain-imucam-tum.yaml b/config/camchain-imucam-tum.yaml new file mode 100644 index 0000000..7edbd87 --- /dev/null +++ b/config/camchain-imucam-tum.yaml @@ -0,0 +1,36 @@ +cam0: + T_cam_imu: + [-0.9995378259923383, 0.02917807204183088, -0.008530798463872679, 0.047094247958417004, + 0.007526588843243184, -0.03435493139706542, -0.9993813532126198, -0.04788273017221637, + -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, + -0.001662284667857643] + distortion_model: equidistant + intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506] + resolution: [1024, 1024] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: + [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335, + 0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889, + -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645, + 0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335, + 0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977, + 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, + 0.0, 0.0, 0.0, 1.0] + camera_model: pinhole + distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, + -0.002347858896562788] + distortion_model: equidistant + intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407] + resolution: [1024, 1024] + rostopic: /cam1/image_raw +T_imu_body: + [1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000] \ No newline at end of file diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 3793d49..492cba3 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -59,11 +59,11 @@ struct Feature { // Constructors for the struct. Feature(): id(0), position(Eigen::Vector3d::Zero()), - is_initialized(false) {} + is_initialized(false), is_anchored(false) {} Feature(const FeatureIDType& new_id): id(new_id), position(Eigen::Vector3d::Zero()), - is_initialized(false) {} + is_initialized(false), is_anchored(false) {} /* * @brief cost Compute the cost of the camera observations @@ -217,7 +217,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p, // A indicator to show if the 3d postion of the feature // has been initialized or not. bool is_initialized; - + bool is_anchored; // Noise for a normalized feature measurement. static double observation_noise; @@ -380,6 +380,8 @@ bool Feature::estimate_FrameIrradiance( double b_A = 0; double a_l =frameExposureTime_ms; double b_l = 0; + printf("frames: %lld, %lld\n", anchor->first, cam_state_id); + printf("exposure: %f, %f\n", a_A, a_l); for (double anchorPixel : anchorPatch) { float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l; @@ -396,6 +398,8 @@ bool Feature::FrameIrradiance( std::vector& anchorPatch_measurement) const { //project every point in anchorPatch_3d. + if(!is_anchored) + printf("not anchored!\n"); for (auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point); @@ -471,12 +475,19 @@ bool Feature::initializeAnchor( cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; - + printf("initializing anchor\n"); + if(u - n < 0 || u + n >= cam.resolution(0) || v - n < 0 || v + n >= cam.resolution(1)) + { + printf("no good: \n"); + printf("%f, %f\n", u, v); + return false; + } //for NxN patch pixels around feature for(double u_run = u - n; u_run <= u + n; u_run = u_run + 1) { for(double v_run = v - n; v_run <= v + n; v_run = v_run + 1) { + printf("ADDING\n"); // add irradiance information anchorPatch.push_back((double)anchorImage.at((int)u_run,(int)v_run)); @@ -489,8 +500,8 @@ bool Feature::initializeAnchor( anchorPatch_3d.push_back(Npose); } } - - //TODO test if NxN patch can be selected + printf("set to true!!!\n"); + is_anchored = true; return true; } diff --git a/launch/image_processor_tum.launch b/launch/image_processor_tum.launch new file mode 100644 index 0000000..d2a02e4 --- /dev/null +++ b/launch/image_processor_tum.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch new file mode 100644 index 0000000..247e824 --- /dev/null +++ b/launch/msckf_vio_tum.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 4e26618..d35bb16 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -390,7 +390,6 @@ void ImageProcessor::predictFeatureTracking( const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics, vector& compensated_pts) { - // Return directly if there are no input features. if (input_pts.size() == 0) { compensated_pts.clear(); @@ -421,7 +420,6 @@ void ImageProcessor::trackFeatures() { cam0_curr_img_ptr->image.rows / processor_config.grid_row; static int grid_width = cam0_curr_img_ptr->image.cols / processor_config.grid_col; - // Compute a rough relative rotation which takes a vector // from the previous frame to the current frame. Matx33f cam0_R_p_c; @@ -611,7 +609,6 @@ void ImageProcessor::stereoMatch( const vector& cam0_points, vector& cam1_points, vector& inlier_markers) { - if (cam0_points.size() == 0) return; if(cam1_points.size() == 0) { @@ -700,8 +697,8 @@ void ImageProcessor::addNewFeatures() { cam0_curr_img_ptr->image.rows / processor_config.grid_row; static int grid_width = cam0_curr_img_ptr->image.cols / processor_config.grid_col; - // Create a mask to avoid redetecting existing features. + Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1)); for (const auto& features : *curr_features_ptr) { @@ -721,7 +718,6 @@ void ImageProcessor::addNewFeatures() { mask(row_range, col_range) = 0; } } - // Detect new features. vector new_features(0); detector_ptr->detect(curr_img, new_features, mask); @@ -736,7 +732,6 @@ void ImageProcessor::addNewFeatures() { new_feature_sieve[ row*processor_config.grid_col+col].push_back(feature); } - new_features.clear(); for (auto& item : new_feature_sieve) { if (item.size() > processor_config.grid_max_feature_num) { @@ -749,7 +744,6 @@ void ImageProcessor::addNewFeatures() { } int detected_new_features = new_features.size(); - // Find the stereo matched points for the newly // detected features. vector cam0_points(new_features.size()); @@ -777,7 +771,6 @@ void ImageProcessor::addNewFeatures() { static_cast(detected_new_features) < 0.1) ROS_WARN("Images at [%f] seems unsynced...", cam0_curr_img_ptr->header.stamp.toSec()); - // Group the features into grids GridFeatures grid_new_features; for (int code = 0; code < @@ -799,7 +792,6 @@ void ImageProcessor::addNewFeatures() { new_feature.cam1_point = cam1_point; grid_new_features[code].push_back(new_feature); } - // Sort the new features in each grid based on its response. for (auto& item : grid_new_features) std::sort(item.second.begin(), item.second.end(), @@ -849,73 +841,6 @@ void ImageProcessor::pruneGridFeatures() { return; } -void ImageProcessor::undistortPoints( - const vector& pts_in, - const cv::Vec4d& intrinsics, - const string& distortion_model, - const cv::Vec4d& distortion_coeffs, - vector& pts_out, - const cv::Matx33d &rectification_matrix, - const cv::Vec4d &new_intrinsics) { - - if (pts_in.size() == 0) return; - - const cv::Matx33d K( - intrinsics[0], 0.0, intrinsics[2], - 0.0, intrinsics[1], intrinsics[3], - 0.0, 0.0, 1.0); - - const cv::Matx33d K_new( - new_intrinsics[0], 0.0, new_intrinsics[2], - 0.0, new_intrinsics[1], new_intrinsics[3], - 0.0, 0.0, 1.0); - - if (distortion_model == "radtan") { - cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } else if (distortion_model == "equidistant") { - cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } else { - ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...", - distortion_model.c_str()); - cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs, - rectification_matrix, K_new); - } - - return; -} - -vector ImageProcessor::distortPoints( - const vector& pts_in, - const cv::Vec4d& intrinsics, - const string& distortion_model, - const cv::Vec4d& distortion_coeffs) { - - const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2], - 0.0, intrinsics[1], intrinsics[3], - 0.0, 0.0, 1.0); - - vector pts_out; - if (distortion_model == "radtan") { - vector homogenous_pts; - cv::convertPointsToHomogeneous(pts_in, homogenous_pts); - cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, - distortion_coeffs, pts_out); - } else if (distortion_model == "equidistant") { - cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs); - } else { - ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...", - distortion_model.c_str()); - vector homogenous_pts; - cv::convertPointsToHomogeneous(pts_in, homogenous_pts); - cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K, - distortion_coeffs, pts_out); - } - - return pts_out; -} - void ImageProcessor::integrateImuData( Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) { // Find the start and the end limit within the imu msg buffer. @@ -967,7 +892,6 @@ void ImageProcessor::integrateImuData( void ImageProcessor::rescalePoints( vector& pts1, vector& pts2, float& scaling_factor) { - scaling_factor = 0.0f; for (int i = 0; i < pts1.size(); ++i) { @@ -1232,7 +1156,6 @@ void ImageProcessor::twoPointRansac( } void ImageProcessor::publish() { - // Publish features. CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement); feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index bc38a4e..0d876d3 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -306,7 +306,6 @@ void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - // Return if the gravity vector has not been set. if (!is_gravity_set) return; @@ -344,7 +343,7 @@ void MsckfVio::imageCallback( // Add new images to moving window start_time = ros::Time::now(); - //manageMovingWindow(cam0_img, cam1_img, feature_msg); + manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); @@ -398,16 +397,16 @@ void MsckfVio::manageMovingWindow( 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) / 1000; - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000; + 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 > 500) - cam0_moving_window[state_server.imu_state.id].exposureTime_ms = 500; - if(cam1_moving_window[state_server.imu_state.id].exposureTime_ms > 500) - cam1_moving_window[state_server.imu_state.id].exposureTime_ms = 500; + 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, @@ -981,6 +980,8 @@ void MsckfVio::PhotometricMeasurementJacobian( r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2), p_c1(0)/p_c1(2), p_c1(1)/p_c1(2)); + printf("-----\n"); + //estimate photometric measurement std::vector estimate_photo_z; feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); @@ -990,9 +991,9 @@ void MsckfVio::PhotometricMeasurementJacobian( for(int i = 0; i < photo_z.size(); i++) photo_r.push_back(photo_z[i] - estimate_photo_z[i]); - // printf("-----\n"); - // for(int i = 0; i < photo_z.size(); i++) - // printf("%.4f - %.4f\n", photo_z[i], estimate_photo_z[i]); + + for(int i = 0; i < photo_z.size(); i++) + printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]); photo_z.clear(); return; @@ -1341,6 +1342,9 @@ void MsckfVio::removeLostFeatures() { continue; } } + } + if(!feature.is_anchored) + { if(!feature.initializeAnchor(cam0_moving_window, cam0)) { invalid_feature_ids.push_back(feature.id); @@ -1477,7 +1481,6 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(involved_cam_state_ids[0]); continue; } - if (!feature.is_initialized) { // Check if the feature can be initialize. if (!feature.checkMotion(state_server.cam_states)) { @@ -1494,6 +1497,9 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } + } + if(!feature.is_anchored) + { if(!feature.initializeAnchor(cam0_moving_window, cam0)) { for (const auto& cam_id : involved_cam_state_ids) @@ -1501,7 +1507,6 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } @@ -1512,7 +1517,6 @@ void MsckfVio::pruneCamStateBuffer() { 21+6*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - ros::Time start_time = ros::Time::now(); for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -1539,8 +1543,6 @@ void MsckfVio::pruneCamStateBuffer() { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - double anchorPrune_processing_time = (ros::Time::now()-start_time).toSec(); - printf("FeatureJacobian Time: %f\n", anchorPrune_processing_time); H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr);