diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 995c5a3..250dfac 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,32 +202,56 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian(const FeatureIDType& feature_id, + void featureJacobian( + const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void twodotMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); + + bool ConstructJacobians( + Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl); + + bool PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + Eigen::VectorXd& r); + + bool PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj); bool PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - Eigen::MatrixXd& H_x, - Eigen::MatrixXd& H_y, - Eigen::VectorXd& r); + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + Eigen::MatrixXd& H_x, + Eigen::MatrixXd& H_y, + Eigen::VectorXd& r); void twodotFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); bool PhotometricFeatureJacobian( - const FeatureIDType& feature_id, - const std::vector& cam_state_ids, - Eigen::MatrixXd& H_x, Eigen::VectorXd& r); + const FeatureIDType& feature_id, + const std::vector& cam_state_ids, + Eigen::MatrixXd& H_x, Eigen::VectorXd& r); void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); void measurementUpdate(const Eigen::MatrixXd& H, @@ -263,6 +287,13 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + double eval_time; + + IMUState timed_old_imu_state; + IMUState timed_old_true_state; + + IMUState old_imu_state; + IMUState old_true_state; // change in position Eigen::Vector3d delta_position; diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..89112e1 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 168eebc..6867743 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -223,8 +223,8 @@ void ImageProcessor::stereoCallback( image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - ROS_INFO("Publishing: %f", - (ros::Time::now()-start_time).toSec()); + //ROS_INFO("Publishing: %f", + // (ros::Time::now()-start_time).toSec()); // Build the image pyramids once since they're used at multiple places createImagePyramids(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 22b5f4b..51a56b0 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -260,6 +260,7 @@ bool MsckfVio::createRosIO() { // activating bag playing parameter, for debugging nh.setParam("/play_bag", true); + eval_time = 0; odom_pub = nh.advertise("odom", 10); truth_odom_pub = nh.advertise("true_odom", 10); feature_pub = nh.advertise( @@ -412,6 +413,10 @@ void MsckfVio::imageCallback( return; } + old_imu_state = state_server.imu_state; + old_true_state = true_state_server.imu_state; + + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -419,7 +424,6 @@ void MsckfVio::imageCallback( is_first_img = false; state_server.imu_state.time = feature_msg->header.stamp.toSec(); } - static double max_processing_time = 0.0; static int critical_time_cntr = 0; double processing_start_time = ros::Time::now().toSec(); @@ -427,24 +431,14 @@ void MsckfVio::imageCallback( // Propogate the IMU state. // that are received before the image feature_msg. ros::Time start_time = ros::Time::now(); - - - batchImuProcessing(feature_msg->header.stamp.toSec()); - - if(GROUNDTRUTH) - { - state_server.imu_state.position = true_state_server.imu_state.position; - state_server.imu_state.orientation = true_state_server.imu_state.orientation; - state_server.imu_state.position_null = true_state_server.imu_state.position_null; - state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null; - state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0; - state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu; - } + nh.param("FILTER", FILTER, 0); + + batchImuProcessing(feature_msg->header.stamp.toSec()); + double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -453,7 +447,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -462,7 +456,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -470,14 +464,14 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - cout << "4" << endl; + // cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); - cout << "5" << endl; + // cout << "5" << endl; start_time = ros::Time::now(); pruneLastCamStateBuffer(); double prune_cam_states_time = ( @@ -487,7 +481,7 @@ void MsckfVio::imageCallback( batchTruthProcessing(feature_msg->header.stamp.toSec()); - cout << "6" << endl; + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -504,18 +498,18 @@ void MsckfVio::imageCallback( ++critical_time_cntr; ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", processing_time, critical_time_cntr); - printf("IMU processing time: %f/%f\n", - imu_processing_time, imu_processing_time/processing_time); - printf("State augmentation time: %f/%f\n", - state_augmentation_time, state_augmentation_time/processing_time); - printf("Add observations time: %f/%f\n", - add_observations_time, add_observations_time/processing_time); - printf("Remove lost features time: %f/%f\n", - remove_lost_features_time, remove_lost_features_time/processing_time); - printf("Remove camera states time: %f/%f\n", - prune_cam_states_time, prune_cam_states_time/processing_time); - printf("Publish time: %f/%f\n", - publish_time, publish_time/processing_time); + //printf("IMU processing time: %f/%f\n", + // imu_processing_time, imu_processing_time/processing_time); + //printf("State augmentation time: %f/%f\n", + // state_augmentation_time, state_augmentation_time/processing_time); + //printf("Add observations time: %f/%f\n", + // add_observations_time, add_observations_time/processing_time); + //printf("Remove lost features time: %f/%f\n", + // remove_lost_features_time, remove_lost_features_time/processing_time); + //printf("Remove camera states time: %f/%f\n", + // prune_cam_states_time, prune_cam_states_time/processing_time); + //printf("Publish time: %f/%f\n", + // publish_time, publish_time/processing_time); } if(STREAMPAUSE) @@ -806,8 +800,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; - const IMUState old_true_state = true_state_server.imu_state; - for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,22 +831,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + /* + // calculate change + delta_position = state_server.imu_state.position - old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); // calculate error in position // calculate error in change - // calculate change Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); Eigen::Vector3d error_delta_position = delta_true_position - delta_position; Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; - - double dx = (error_delta_position[0]/delta_true_position[0]); - double dy = (error_delta_position[1]/delta_true_position[1]); - double dz = (error_delta_position[2]/delta_true_position[2]); - cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + + cout << "relative pos error: " << relerr * 100 << "%" << endl; + cout << "relative ori error: " << relOerr * 100 << "%" << endl; + //cout << "absolute pos error: " << + */ + if (eval_time + 1 < ros::Time::now().toSec()) + { + + // calculate change + delta_position = state_server.imu_state.position - timed_old_imu_state.position; + Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation); + delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2); + // calculate error in position + + // calculate error in change + + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position; + Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation); + Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2); + + double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/ + sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2])); + + double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/ + sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2])); + + // cout << "relative pos error: " << relerr * 100 << "%" << endl; + // cout << "relative ori error: " << relOerr * 100 << "%" << endl; + + timed_old_imu_state = state_server.imu_state; + timed_old_true_state = true_state_server.imu_state; + eval_time = ros::Time::now().toSec(); + } } void MsckfVio::processTruthtoIMU(const double& time, @@ -1477,17 +1523,60 @@ void MsckfVio::twodotFeatureJacobian( return; } - - -bool MsckfVio::PhotometricMeasurementJacobian( - const StateIDType& cam_state_id, - const FeatureIDType& feature_id, - MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +bool MsckfVio::PhotometricPatchPointResidual( + const StateIDType& cam_state_id, + const Feature& feature, + VectorXd& r) { - // Prepare all the required data. + VectorXd r_photo = VectorXd::Zero(N*N); + int count = 0; + auto frame = cam0.moving_window.find(cam_state_id)->second.image; + const CAMState& cam_state = state_server.cam_states[cam_state_id]; - const Feature& feature = map_server[feature_id]; + + //estimate photometric measurement + std::vector estimate_irradiance; + std::vector estimate_photo_z; + std::vector photo_z; + + IlluminationParameter estimated_illumination; + feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); + for (auto& estimate_irradiance_j : estimate_irradiance) + estimate_photo_z.push_back (estimate_irradiance_j * + estimated_illumination.frame_gain * estimated_illumination.feature_gain + + estimated_illumination.frame_bias + estimated_illumination.feature_bias); + + + for(auto point : feature.anchorPatch_3d) + { + + + cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); + // test if projection is inside frame + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + return false; + //add observation + photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); + + //calculate photom. residual + r_photo(count) = photo_z[count] - estimate_photo_z[count]; + count++; + } + r = r_photo; + return true; +} + +// generates the jacobian of one patch point regarding rho, anchor and current frame +bool MsckfVio::PhotometricPatchPointJacobian( + const CAMState& cam_state, + const Feature& feature, + Eigen::Vector3d point, + int count, + Eigen::Matrix& H_rhoj, + Eigen::Matrix& H_plj, + Eigen::Matrix& H_pAj) +{ const StateIDType anchor_state_id = feature.observations.begin()->first; const CAMState anchor_state = state_server.cam_states[anchor_state_id]; @@ -1501,10 +1590,7 @@ bool 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(); - - //photometric observation - std::vector photo_z; - VectorXd r_photo = VectorXd::Zero(N*N); + // individual Jacobians Matrix dI_dhj = Matrix::Zero(); Matrix dh_dCpij = Matrix::Zero(); @@ -1517,6 +1603,69 @@ bool MsckfVio::PhotometricMeasurementJacobian( Matrix dCpij_dCGtheta = Matrix::Zero(); Matrix dCpij_dGpC = Matrix::Zero(); + double dx, dy; + + Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w); + cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + + + // calculate derivation for anchor frame, use position for derivation calculation + // frame derivative calculated convoluting with kernel [-1, 0, 1] + dx = feature.cvKernel(p_in_anchor, "Sobel_x"); + dy = feature.cvKernel(p_in_anchor, "Sobel_y"); + + dI_dhj(0, 0) = dx * cam0.intrinsics[0]; + dI_dhj(0, 1) = dy * cam0.intrinsics[1]; + + //dh / d{}^Cp_{ij} + dh_dCpij(0, 0) = 1 / p_c0(2); + dh_dCpij(1, 1) = 1 / p_c0(2); + 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 * dCpij_dGpij; + + //dh / d X_{pl} + 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; + // Isometry T_anchor_w takes a vector in anchor frame to world frame + 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)); + + dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() + * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), + feature.anchorPatch_ideal[count].y/(rho), + 1/(rho))); + dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); + + // Intermediate Jakobians + H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 + H_plj = dI_dhj * dh_dXplj; // 1 x 6 + H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 + + return true; +} + +bool MsckfVio::PhotometricMeasurementJacobian( + const StateIDType& cam_state_id, + const FeatureIDType& feature_id, + MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) +{ + + // Prepare all the required data. + const CAMState& cam_state = state_server.cam_states[cam_state_id]; + const Feature& feature = map_server[feature_id]; + + //photometric observation + VectorXd r_photo = VectorXd::Zero(N*N); + // one line of the NxN Jacobians Eigen::Matrix H_rhoj; Eigen::Matrix H_plj; @@ -1528,89 +1677,18 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(N*N, 6); auto frame = cam0.moving_window.find(cam_state_id)->second.image; - auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image; - //observation - const Vector4d& z = feature.observations.find(cam_state_id)->second; - //estimate photometric measurement - std::vector estimate_irradiance; - std::vector estimate_photo_z; - IlluminationParameter estimated_illumination; - feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination); - - // calculated here, because we need true 'estimate_irradiance' later for jacobi - for (auto& estimate_irradiance_j : estimate_irradiance) - estimate_photo_z.push_back (estimate_irradiance_j * - estimated_illumination.frame_gain * estimated_illumination.feature_gain + - estimated_illumination.frame_bias + estimated_illumination.feature_bias); + // calcualte residual of patch + PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + // calculate jacobian for patch int count = 0; - double dx, dy; - for (auto point : feature.anchorPatch_3d) { - //cout << "____feature-measurement_____\n" << endl; - 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); - cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); - - if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) - { - cout << "skipped" << endl; - return false; - } - //add observation - photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); - - //calculate photom. residual - r_photo(count) = photo_z[count] - estimate_photo_z[count]; - //cout << "residual: " << photo_r.back() << endl; - - // calculate derivation for anchor frame, use position for derivation calculation - // frame derivative calculated convoluting with kernel [-1, 0, 1] - - dx = feature.cvKernel(p_in_anchor, "Sobel_x"); - dy = feature.cvKernel(p_in_anchor, "Sobel_y"); - - // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); - // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); - - dI_dhj(0, 0) = dx * cam0.intrinsics[0]; - dI_dhj(0, 1) = dy * cam0.intrinsics[1]; - - //dh / d{}^Cp_{ij} - dh_dCpij(0, 0) = 1 / p_c0(2); - dh_dCpij(1, 1) = 1 / p_c0(2); - 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 * dCpij_dGpij; - - //dh / d X_{pl} - 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; - // Isometry T_anchor_w takes a vector in anchor frame to world frame - 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)); - - dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear() - * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), - feature.anchorPatch_ideal[count].y/(rho), - 1/(rho))); - dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); - - // Intermediate Jakobians - H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dI_dhj * dh_dXplj; // 1 x 6 - H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - + // get jacobi of single point in patch + PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj); + + // stack point into entire jacobi H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; @@ -1618,48 +1696,31 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - cout << "done" << endl; + // construct the jacobian structure needed for nullspacing MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); - // set anchor Jakobi - // get position of anchor in cam states + ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl); - auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); - int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); - H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; - - // set frame Jakobi - //get position of current frame in cam states - auto cam_state_iter = state_server.cam_states.find(cam_state_id); - int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); - - // set jakobi of state - H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; - - // set ones for irradiance bias - // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); - - // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); - - // TODO make this calculation more fluent - - /*for(int i = 0; i< N*N; i++) - H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - */ - - H_yl.block(0, N*N, N*N, 1) = -H_rho; + // set to return values H_x = H_xl; H_y = H_yl; - r = r_photo; - std::stringstream ss; - ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; if(PRINTIMAGES) - { + { + // pregenerating information for visualization + std::stringstream ss; + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; + + // visualizing functions feature.MarkerGeneration(marker_pub, state_server.cam_states); feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); @@ -1668,6 +1729,43 @@ bool MsckfVio::PhotometricMeasurementJacobian( return true; } +bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho, + Eigen::MatrixXd& H_pl, + Eigen::MatrixXd& H_pA, + const Feature& feature, + const StateIDType& cam_state_id, + Eigen::MatrixXd& H_xl, + Eigen::MatrixXd& H_yl) +{ + // get position of anchor in cam states + auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first); + int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor); + // set anchor Jakobi + H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; + + //get position of current frame in cam states + auto cam_state_iter = state_server.cam_states.find(cam_state_id); + // set frame Jakobi + int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter); + + // set jakobi of state + H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; + + // set ones for irradiance bias + // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + + // set irradiance error Block + H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); + + + /*for(int i = 0; i< N*N; i++) + H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; + */ + + H_yl.block(0, N*N, N*N, 1) = -H_rho; + +} + bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, @@ -1717,11 +1815,9 @@ bool MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - if(stack_cntr == 0) - { - cout << "skip feature" << endl; + if(stack_cntr < 2*N*N) return false; - } + // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -2027,8 +2123,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { VectorXd delta_x = K * r; - cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; + // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; + cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; if(FILTER != 0) return; @@ -2053,9 +2149,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { myfile.close(); } - delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); - delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); - // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2239,10 +2332,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Perform QR decompostion on H_sparse. SPQR > spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); - spqr_helper.compute(H_sparse); + cout << "comp" << endl; + spqr_helper.compute(H_sparse); + cout << "done" << endl; MatrixXd H_temp; VectorXd r_temp; + (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp); (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp); @@ -2261,6 +2357,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); // Compute the error of the state. @@ -2271,28 +2368,27 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r if(PRINTIMAGES) { - //octave - ofstream myfile; - - myfile.open("/home/raphael/dev/octave/measurement2octave"); - myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" - << "# name: K\n" - << "# type: matrix\n" - << "# rows: " << K.rows() << "\n" - << "# columns: " << K.cols() << "\n" - << K << endl; + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; - myfile << "# name: r\n" - << "# type: matrix\n" - << "# rows: " << r.rows() << "\n" - << "# columns: " << r.cols() << "\n" - << r << endl; + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; - myfile.close(); + myfile.close(); } - delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); - delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2349,8 +2445,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - MatrixXd P1 = H * state_server.state_cov * H.transpose(); + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); @@ -2363,10 +2459,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) if (chi_squared_test_table[dof] == 0) return false; if (gamma < chi_squared_test_table[dof]) { - //cout << "passed" << endl; + // cout << "passed" << endl; return true; } else { - //cout << "failed" << endl; + // cout << "failed" << endl; return false; } } @@ -2465,16 +2561,15 @@ void MsckfVio::removeLostFeatures() { VectorXd pr_j; MatrixXd twoH_xj; VectorXd twor_j; - - /* - if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ + } featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2499,10 +2594,11 @@ void MsckfVio::removeLostFeatures() { { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - + photometricMeasurementUpdate(pH_x, pr); } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2648,17 +2744,15 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - /* - if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); - { + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ - + } featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2679,26 +2773,26 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } - if (pstack_cntr) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); pr.conservativeResize(pstack_cntr); - photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - // Perform measurement update. - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - // Perform measurement update. - - + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2831,7 +2925,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - /* + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { @@ -2839,7 +2933,8 @@ void MsckfVio::pruneCamStateBuffer() { pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - }*/ + } + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2860,23 +2955,24 @@ void MsckfVio::pruneCamStateBuffer() { } - if(pstack_cntr > 0) + + if(pstack_cntr) { pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); - + pr.conservativeResize(pstack_cntr); photometricMeasurementUpdate(pH_x, pr); } H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); - - // Perform measurement update. + + // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(),