added projection into feature observations camera states
This commit is contained in:
@ -443,12 +443,14 @@ void MsckfVio::initializeGravityAndBias() {
|
||||
// is consistent with the inertial frame.
|
||||
double gravity_norm = gravity_imu.norm();
|
||||
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
|
||||
|
||||
|
||||
Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
|
||||
gravity_imu, -IMUState::gravity);
|
||||
state_server.imu_state.orientation =
|
||||
rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());
|
||||
|
||||
// printf("gravity Norm %f\n", gravity_norm);
|
||||
// printf("linear_acc %f, %f, %f\n", gravity_imu(0), gravity_imu(1), gravity_imu(2));
|
||||
// printf("quaterniond: %f, %f, %f, %f\n", q0_i_w.w(), q0_i_w.x(), q0_i_w.y(), q0_i_w.z());
|
||||
return;
|
||||
}
|
||||
|
||||
@ -803,6 +805,7 @@ 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
|
||||
@ -874,6 +877,157 @@ void MsckfVio::addFeatureObservations(
|
||||
return;
|
||||
}
|
||||
|
||||
void MsckfVio::PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
|
||||
|
||||
// Prepare all the required data.
|
||||
const CAMState& cam_state = state_server.cam_states[cam_state_id];
|
||||
const Feature& feature = map_server[feature_id];
|
||||
|
||||
// Cam0 pose.
|
||||
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
|
||||
const Vector3d& t_c0_w = cam_state.position;
|
||||
|
||||
// Cam1 pose.
|
||||
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
|
||||
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;
|
||||
|
||||
//observation
|
||||
const Vector4d& z = feature.observations.find(cam_state_id)->second;
|
||||
|
||||
//photometric observation
|
||||
std::vector<uint8_t> 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
|
||||
// the cam0 and cam1 frame.
|
||||
Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
|
||||
Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);
|
||||
|
||||
|
||||
//compute resulting esimtated position in image
|
||||
cv::Point2f out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
|
||||
std::vector<cv::Point2f> out_v;
|
||||
out_v.push_back(out_p);
|
||||
|
||||
/*
|
||||
//prints the feature projection in pixel space
|
||||
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
|
||||
cam0_intrinsics,
|
||||
cam0_distortion_model,
|
||||
cam0_distortion_coeffs);
|
||||
printf("projection: %f, %f\n", my_p[0].x, my_p[0].y);
|
||||
*/
|
||||
|
||||
// Compute the Jacobians.
|
||||
Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
|
||||
dz_dpc0(0, 0) = 1 / p_c0(2);
|
||||
dz_dpc0(1, 1) = 1 / p_c0(2);
|
||||
dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
|
||||
dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
|
||||
|
||||
Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
|
||||
dz_dpc1(2, 0) = 1 / p_c1(2);
|
||||
dz_dpc1(3, 1) = 1 / p_c1(2);
|
||||
dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
|
||||
dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
|
||||
|
||||
Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
|
||||
dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
|
||||
dpc0_dxc.rightCols(3) = -R_w_c0;
|
||||
|
||||
Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
|
||||
dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
|
||||
dpc1_dxc.rightCols(3) = -R_w_c1;
|
||||
|
||||
Matrix3d dpc0_dpg = R_w_c0;
|
||||
Matrix3d dpc1_dpg = R_w_c1;
|
||||
|
||||
H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
|
||||
H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
|
||||
|
||||
// Modifty the measurement Jacobian to ensure
|
||||
// observability constrain.
|
||||
Matrix<double, 4, 6> A = H_x;
|
||||
Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
|
||||
u.block<3, 1>(0, 0) = quaternionToRotation(
|
||||
cam_state.orientation_null) * IMUState::gravity;
|
||||
u.block<3, 1>(3, 0) = skewSymmetric(
|
||||
p_w-cam_state.position_null) * IMUState::gravity;
|
||||
H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
|
||||
H_f = -H_x.block<4, 3>(0, 3);
|
||||
|
||||
// Compute the residual.
|
||||
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));
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void MsckfVio::PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r) {
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
// Check how many camera states in the provided camera
|
||||
// id camera has actually seen this feature.
|
||||
vector<StateIDType> valid_cam_state_ids(0);
|
||||
for (const auto& cam_id : cam_state_ids) {
|
||||
if (feature.observations.find(cam_id) ==
|
||||
feature.observations.end()) continue;
|
||||
|
||||
valid_cam_state_ids.push_back(cam_id);
|
||||
}
|
||||
|
||||
int jacobian_row_size = 0;
|
||||
jacobian_row_size = 4 * valid_cam_state_ids.size();
|
||||
|
||||
MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
|
||||
21+state_server.cam_states.size()*6);
|
||||
MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
|
||||
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
|
||||
for (const auto& cam_id : valid_cam_state_ids) {
|
||||
|
||||
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
|
||||
Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
|
||||
Vector4d r_i = Vector4d::Zero();
|
||||
PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);
|
||||
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_id);
|
||||
int cam_state_cntr = std::distance(
|
||||
state_server.cam_states.begin(), cam_state_iter);
|
||||
|
||||
// Stack the Jacobians.
|
||||
H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
|
||||
H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
|
||||
r_j.segment<4>(stack_cntr) = r_i;
|
||||
stack_cntr += 4;
|
||||
}
|
||||
|
||||
// Project the residual and Jacobians onto the nullspace
|
||||
// of H_fj.
|
||||
JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
|
||||
MatrixXd A = svd_helper.matrixU().rightCols(
|
||||
jacobian_row_size - 3);
|
||||
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void MsckfVio::measurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
@ -1199,7 +1353,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
|
||||
MatrixXd H_xj;
|
||||
VectorXd r_j;
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
@ -1316,6 +1470,13 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
continue;
|
||||
}
|
||||
|
||||
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
|
||||
}
|
||||
|
||||
@ -1342,7 +1503,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
MatrixXd H_xj;
|
||||
VectorXd r_j;
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
|
||||
Reference in New Issue
Block a user