tested bug: filter converges longer without distortion effects; reason probably general formulation inconsistencies
This commit is contained in:
@ -1232,12 +1232,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
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();
|
||||
|
||||
|
||||
//photometric observation
|
||||
std::vector<double> photo_z;
|
||||
|
||||
@ -1316,16 +1310,21 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
//calculate residual
|
||||
|
||||
//project pixel point to 'pure' position
|
||||
cv::Point2f und_p_in_c0;
|
||||
image_handler::undistortPoint(p_in_c0,
|
||||
cam0.intrinsics,
|
||||
cam0.distortion_model,
|
||||
0,
|
||||
cam0.distortion_coeffs,
|
||||
und_p_in_c0);
|
||||
|
||||
r_i[0] = z[0] - und_p_in_c0.x;
|
||||
r_i[1] = z[1] - und_p_in_c0.y;
|
||||
|
||||
//cout << "comp\n" << p_c0(0)/p_c0(2) << ":" << und_p_in_c0.x << endl;
|
||||
//cout << p_c0(1)/p_c0(2) << ":" << und_p_in_c0.y << endl;
|
||||
|
||||
|
||||
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
|
||||
|
||||
// set anchor Jakobi
|
||||
@ -1346,7 +1345,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_x = H_xl;
|
||||
H_y = H_rho;
|
||||
r = r_i;
|
||||
cout << "h for patch done" << endl;
|
||||
|
||||
//TODO make this more fluent as well
|
||||
if(PRINTIMAGES)
|
||||
|
Reference in New Issue
Block a user