diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 3cfa49c..e56b6c9 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -15,6 +15,21 @@ #include "imu_state.h" namespace msckf_vio { + +struct IlluminationParameter{ + double frame_bias; + double frame_gain; + double feature_bias; + double feature_gain; +}; + +struct CameraCalibration{ + std::string distortion_model; + cv::Vec2i resolution; + cv::Vec4d intrinsics; + cv::Vec4d distortion_coeffs; +}; + /* * @brief CAMState Stored camera state in order to * form measurement model. @@ -35,7 +50,7 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; - //Illumination illumination; + IlluminationParameter illumination; // These two variables should have the same physical // interpretation with `orientation` and `position`. diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h index 086c5a9..638fbcd 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -22,6 +22,8 @@ #include #include +#include "cam_state.h" + namespace msckf_vio { /* @@ -332,15 +334,8 @@ private: std::vector imu_msg_buffer; // Camera calibration parameters - std::string cam0_distortion_model; - cv::Vec2i cam0_resolution; - cv::Vec4d cam0_intrinsics; - cv::Vec4d cam0_distortion_coeffs; - - std::string cam1_distortion_model; - cv::Vec2i cam1_resolution; - cv::Vec4d cam1_intrinsics; - cv::Vec4d cam1_distortion_coeffs; + CameraCalibration cam0; + CameraCalibration cam1; // Take a vector from cam0 frame to the IMU frame. cv::Matx33d R_cam0_imu; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index f857a52..b87349b 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -221,15 +221,9 @@ class MsckfVio { movingWindow cam1_moving_window; // Camera calibration parameters - std::string cam0_distortion_model; - cv::Vec2i cam0_resolution; - cv::Vec4d cam0_intrinsics; - cv::Vec4d cam0_distortion_coeffs; + CameraCalibration cam0; + CameraCalibration cam1; - std::string cam1_distortion_model; - cv::Vec2i cam1_resolution; - cv::Vec4d cam1_intrinsics; - cv::Vec4d cam1_distortion_coeffs; // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/src/image_processor.cpp b/src/image_processor.cpp index cea396d..4e26618 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -44,49 +44,49 @@ ImageProcessor::~ImageProcessor() { bool ImageProcessor::loadParameters() { // Camera calibration parameters nh.param("cam0/distortion_model", - cam0_distortion_model, string("radtan")); + cam0.distortion_model, string("radtan")); nh.param("cam1/distortion_model", - cam1_distortion_model, string("radtan")); + cam1.distortion_model, string("radtan")); vector cam0_resolution_temp(2); nh.getParam("cam0/resolution", cam0_resolution_temp); - cam0_resolution[0] = cam0_resolution_temp[0]; - cam0_resolution[1] = cam0_resolution_temp[1]; + cam0.resolution[0] = cam0_resolution_temp[0]; + cam0.resolution[1] = cam0_resolution_temp[1]; vector cam1_resolution_temp(2); nh.getParam("cam1/resolution", cam1_resolution_temp); - cam1_resolution[0] = cam1_resolution_temp[0]; - cam1_resolution[1] = cam1_resolution_temp[1]; + cam1.resolution[0] = cam1_resolution_temp[0]; + cam1.resolution[1] = cam1_resolution_temp[1]; vector cam0_intrinsics_temp(4); nh.getParam("cam0/intrinsics", cam0_intrinsics_temp); - cam0_intrinsics[0] = cam0_intrinsics_temp[0]; - cam0_intrinsics[1] = cam0_intrinsics_temp[1]; - cam0_intrinsics[2] = cam0_intrinsics_temp[2]; - cam0_intrinsics[3] = cam0_intrinsics_temp[3]; + cam0.intrinsics[0] = cam0_intrinsics_temp[0]; + cam0.intrinsics[1] = cam0_intrinsics_temp[1]; + cam0.intrinsics[2] = cam0_intrinsics_temp[2]; + cam0.intrinsics[3] = cam0_intrinsics_temp[3]; vector cam1_intrinsics_temp(4); nh.getParam("cam1/intrinsics", cam1_intrinsics_temp); - cam1_intrinsics[0] = cam1_intrinsics_temp[0]; - cam1_intrinsics[1] = cam1_intrinsics_temp[1]; - cam1_intrinsics[2] = cam1_intrinsics_temp[2]; - cam1_intrinsics[3] = cam1_intrinsics_temp[3]; + cam1.intrinsics[0] = cam1_intrinsics_temp[0]; + cam1.intrinsics[1] = cam1_intrinsics_temp[1]; + cam1.intrinsics[2] = cam1_intrinsics_temp[2]; + cam1.intrinsics[3] = cam1_intrinsics_temp[3]; vector cam0_distortion_coeffs_temp(4); nh.getParam("cam0/distortion_coeffs", cam0_distortion_coeffs_temp); - cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; - cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; - cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; - cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; + cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; + cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; + cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; + cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; vector cam1_distortion_coeffs_temp(4); nh.getParam("cam1/distortion_coeffs", cam1_distortion_coeffs_temp); - cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; - cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; - cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; - cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; + cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; + cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; + cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; cv::Mat T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu"); cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3))); @@ -124,27 +124,27 @@ bool ImageProcessor::loadParameters() { processor_config.stereo_threshold, 3); ROS_INFO("==========================================="); - ROS_INFO("cam0_resolution: %d, %d", - cam0_resolution[0], cam0_resolution[1]); + ROS_INFO("cam0.resolution: %d, %d", + cam0.resolution[0], cam0.resolution[1]); ROS_INFO("cam0_intrinscs: %f, %f, %f, %f", - cam0_intrinsics[0], cam0_intrinsics[1], - cam0_intrinsics[2], cam0_intrinsics[3]); - ROS_INFO("cam0_distortion_model: %s", - cam0_distortion_model.c_str()); + cam0.intrinsics[0], cam0.intrinsics[1], + cam0.intrinsics[2], cam0.intrinsics[3]); + ROS_INFO("cam0.distortion_model: %s", + cam0.distortion_model.c_str()); ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f", - cam0_distortion_coeffs[0], cam0_distortion_coeffs[1], - cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]); + cam0.distortion_coeffs[0], cam0.distortion_coeffs[1], + cam0.distortion_coeffs[2], cam0.distortion_coeffs[3]); - ROS_INFO("cam1_resolution: %d, %d", - cam1_resolution[0], cam1_resolution[1]); + ROS_INFO("cam1.resolution: %d, %d", + cam1.resolution[0], cam1.resolution[1]); ROS_INFO("cam1_intrinscs: %f, %f, %f, %f", - cam1_intrinsics[0], cam1_intrinsics[1], - cam1_intrinsics[2], cam1_intrinsics[3]); - ROS_INFO("cam1_distortion_model: %s", - cam1_distortion_model.c_str()); + cam1.intrinsics[0], cam1.intrinsics[1], + cam1.intrinsics[2], cam1.intrinsics[3]); + ROS_INFO("cam1.distortion_model: %s", + cam1.distortion_model.c_str()); ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f", - cam1_distortion_coeffs[0], cam1_distortion_coeffs[1], - cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]); + cam1.distortion_coeffs[0], cam1.distortion_coeffs[1], + cam1.distortion_coeffs[2], cam1.distortion_coeffs[3]); cout << R_imu_cam0 << endl; cout << t_imu_cam0.t() << endl; @@ -455,7 +455,7 @@ void ImageProcessor::trackFeatures() { vector track_inliers(0); predictFeatureTracking(prev_cam0_points, - cam0_R_p_c, cam0_intrinsics, curr_cam0_points); + cam0_R_p_c, cam0.intrinsics, curr_cam0_points); calcOpticalFlowPyrLK( prev_cam0_pyramid_, curr_cam0_pyramid_, @@ -550,14 +550,14 @@ void ImageProcessor::trackFeatures() { // Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1. vector cam0_ransac_inliers(0); twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points, - cam0_R_p_c, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, processor_config.ransac_threshold, + cam0_R_p_c, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, processor_config.ransac_threshold, 0.99, cam0_ransac_inliers); vector cam1_ransac_inliers(0); twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points, - cam1_R_p_c, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, processor_config.ransac_threshold, + cam1_R_p_c, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, processor_config.ransac_threshold, 0.99, cam1_ransac_inliers); // Number of features after ransac. @@ -619,12 +619,12 @@ void ImageProcessor::stereoMatch( // rotation from stereo extrinsics const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu; vector cam0_points_undistorted; - image_handler::undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, cam0_points_undistorted, + image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, cam0_points_undistorted, R_cam0_cam1); - cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1_intrinsics, - cam1_distortion_model, cam1_distortion_coeffs); + cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics, + cam1.distortion_model, cam1.distortion_coeffs); } // Track features using LK optical flow method. @@ -665,15 +665,15 @@ void ImageProcessor::stereoMatch( vector cam0_points_undistorted(0); vector cam1_points_undistorted(0); image_handler::undistortPoints( - cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, cam0_points_undistorted); + cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, cam0_points_undistorted); image_handler::undistortPoints( - cam1_points, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, cam1_points_undistorted); + cam1_points, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, cam1_points_undistorted); double norm_pixel_unit = 4.0 / ( - cam0_intrinsics[0]+cam0_intrinsics[1]+ - cam1_intrinsics[0]+cam1_intrinsics[1]); + cam0.intrinsics[0]+cam0.intrinsics[1]+ + cam1.intrinsics[0]+cam1.intrinsics[1]); for (int i = 0; i < cam0_points_undistorted.size(); ++i) { if (inlier_markers[i] == 0) continue; @@ -1253,11 +1253,11 @@ void ImageProcessor::publish() { vector curr_cam1_points_undistorted(0); image_handler::undistortPoints( - curr_cam0_points, cam0_intrinsics, cam0_distortion_model, - cam0_distortion_coeffs, curr_cam0_points_undistorted); + curr_cam0_points, cam0.intrinsics, cam0.distortion_model, + cam0.distortion_coeffs, curr_cam0_points_undistorted); image_handler::undistortPoints( - curr_cam1_points, cam1_intrinsics, cam1_distortion_model, - cam1_distortion_coeffs, curr_cam1_points_undistorted); + curr_cam1_points, cam1.intrinsics, cam1.distortion_model, + cam1.distortion_coeffs, curr_cam1_points_undistorted); for (int i = 0; i < curr_ids.size(); ++i) { feature_msg_ptr->features.push_back(FeatureMeasurement()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f5391dd..589c2bc 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -119,49 +119,49 @@ bool MsckfVio::loadParameters() { // get camera information (used for back projection) nh.param("cam0/distortion_model", - cam0_distortion_model, string("radtan")); + cam0.distortion_model, string("radtan")); nh.param("cam1/distortion_model", - cam1_distortion_model, string("radtan")); + cam1.distortion_model, string("radtan")); vector cam0_resolution_temp(2); nh.getParam("cam0/resolution", cam0_resolution_temp); - cam0_resolution[0] = cam0_resolution_temp[0]; - cam0_resolution[1] = cam0_resolution_temp[1]; + cam0.resolution[0] = cam0_resolution_temp[0]; + cam0.resolution[1] = cam0_resolution_temp[1]; vector cam1_resolution_temp(2); nh.getParam("cam1/resolution", cam1_resolution_temp); - cam1_resolution[0] = cam1_resolution_temp[0]; - cam1_resolution[1] = cam1_resolution_temp[1]; + cam1.resolution[0] = cam1_resolution_temp[0]; + cam1.resolution[1] = cam1_resolution_temp[1]; vector cam0_intrinsics_temp(4); nh.getParam("cam0/intrinsics", cam0_intrinsics_temp); - cam0_intrinsics[0] = cam0_intrinsics_temp[0]; - cam0_intrinsics[1] = cam0_intrinsics_temp[1]; - cam0_intrinsics[2] = cam0_intrinsics_temp[2]; - cam0_intrinsics[3] = cam0_intrinsics_temp[3]; + cam0.intrinsics[0] = cam0_intrinsics_temp[0]; + cam0.intrinsics[1] = cam0_intrinsics_temp[1]; + cam0.intrinsics[2] = cam0_intrinsics_temp[2]; + cam0.intrinsics[3] = cam0_intrinsics_temp[3]; vector cam1_intrinsics_temp(4); nh.getParam("cam1/intrinsics", cam1_intrinsics_temp); - cam1_intrinsics[0] = cam1_intrinsics_temp[0]; - cam1_intrinsics[1] = cam1_intrinsics_temp[1]; - cam1_intrinsics[2] = cam1_intrinsics_temp[2]; - cam1_intrinsics[3] = cam1_intrinsics_temp[3]; + cam1.intrinsics[0] = cam1_intrinsics_temp[0]; + cam1.intrinsics[1] = cam1_intrinsics_temp[1]; + cam1.intrinsics[2] = cam1_intrinsics_temp[2]; + cam1.intrinsics[3] = cam1_intrinsics_temp[3]; vector cam0_distortion_coeffs_temp(4); nh.getParam("cam0/distortion_coeffs", cam0_distortion_coeffs_temp); - cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; - cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; - cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; - cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; + cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0]; + cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1]; + cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2]; + cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3]; vector cam1_distortion_coeffs_temp(4); nh.getParam("cam1/distortion_coeffs", cam1_distortion_coeffs_temp); - cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; - cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; - cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; - cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; + cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0]; + cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1]; + cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2]; + cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3]; @@ -904,7 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //photometric observation std::vector photo_z; - feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z); + feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z); // Convert the feature position from the world frame to @@ -921,9 +921,9 @@ void MsckfVio::PhotometricMeasurementJacobian( /* //prints the feature projection in pixel space std::vector my_p = image_handler::distortPoints( out_v, - cam0_intrinsics, - cam0_distortion_model, - cam0_distortion_coeffs); + cam0.intrinsics, + cam0.distortion_model, + cam0.distortion_coeffs); printf("projection: %f, %f\n", my_p[0].x, my_p[0].y); */ @@ -1316,7 +1316,7 @@ void MsckfVio::removeLostFeatures() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1470,7 +1470,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs)) + if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id);