minor output changes, added arrows for gradient and residual visualization
This commit is contained in:
@ -1238,6 +1238,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
//photometric observation
|
||||
std::vector<double> photo_z;
|
||||
|
||||
std::vector<double> photo_r;
|
||||
|
||||
// individual Jacobians
|
||||
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
|
||||
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
|
||||
@ -1262,9 +1264,31 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
|
||||
//observation
|
||||
const Vector4d& z = feature.observations.find(cam_state_id)->second;
|
||||
|
||||
//estimate photometric measurement
|
||||
std::vector<double> estimate_irradiance;
|
||||
std::vector<double> 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);
|
||||
|
||||
int count = 0;
|
||||
double dx, dy;
|
||||
|
||||
// gradient visualization parameters
|
||||
cv::Point2f gradientVector(0,0);
|
||||
|
||||
// residual change visualization
|
||||
cv::Point2f residualVector(0,0);
|
||||
double res_sum = 0;
|
||||
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
|
||||
@ -1273,14 +1297,24 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
//add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
// add jacobian
|
||||
//calculate photom. residual
|
||||
photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
|
||||
|
||||
// add jacobians
|
||||
|
||||
// frame derivative calculated convoluting with kernel [-1, 0, 1]
|
||||
dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
|
||||
dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
|
||||
dI_dhj(0, 0) = dx;
|
||||
dI_dhj(0, 1) = dy;
|
||||
|
||||
gradientVector.x += dx;
|
||||
gradientVector.y += dy;
|
||||
|
||||
residualVector.x += dx * photo_r[count];
|
||||
residualVector.y += dy * photo_r[count];
|
||||
res_sum += photo_r[count];
|
||||
|
||||
//dh / d{}^Cp_{ij}
|
||||
dh_dCpij(0, 0) = 1 / p_c0(2);
|
||||
dh_dCpij(1, 1) = 1 / p_c0(2);
|
||||
@ -1320,28 +1354,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
count++;
|
||||
}
|
||||
// calculate residual
|
||||
|
||||
//observation
|
||||
const Vector4d& z = feature.observations.find(cam_state_id)->second;
|
||||
|
||||
//estimate photometric measurement
|
||||
std::vector<double> estimate_irradiance;
|
||||
std::vector<double> 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);
|
||||
|
||||
std::vector<double> photo_r;
|
||||
|
||||
//calculate photom. residual
|
||||
for(int i = 0; i < photo_z.size(); i++)
|
||||
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
|
||||
|
||||
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
|
||||
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
|
||||
@ -1379,12 +1391,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
count = 0;
|
||||
for(auto data : photo_r)
|
||||
r[count++] = data;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
|
||||
}
|
||||
|
||||
return;
|
||||
@ -1481,13 +1494,11 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
H_x = A_null_space.transpose() * H_xi;
|
||||
r = A_null_space.transpose() * r_i;
|
||||
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
|
||||
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
@ -1631,13 +1642,11 @@ void MsckfVio::featureJacobian(
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
|
||||
nh.setParam("/play_bag", false);
|
||||
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
@ -1648,7 +1657,11 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
// complexity as in Equation (28), (29).
|
||||
MatrixXd H_thin;
|
||||
VectorXd r_thin;
|
||||
int augmentationSize = 6;
|
||||
if(PHOTOMETRIC)
|
||||
augmentationSize = 7;
|
||||
|
||||
/*
|
||||
if (H.rows() > H.cols()) {
|
||||
// Convert H to a sparse matrix.
|
||||
SparseMatrix<double> H_sparse = H.sparseView();
|
||||
@ -1663,8 +1676,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
|
||||
(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
|
||||
|
||||
H_thin = H_temp.topRows(21+state_server.cam_states.size()*6);
|
||||
r_thin = r_temp.head(21+state_server.cam_states.size()*6);
|
||||
H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize);
|
||||
r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
|
||||
|
||||
//HouseholderQR<MatrixXd> qr_helper(H);
|
||||
//MatrixXd Q = qr_helper.householderQ();
|
||||
@ -1676,18 +1689,19 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
H_thin = H;
|
||||
r_thin = r;
|
||||
}
|
||||
*/
|
||||
|
||||
// Compute the Kalman gain.
|
||||
const MatrixXd& P = state_server.state_cov;
|
||||
MatrixXd S = H_thin*P*H_thin.transpose() +
|
||||
MatrixXd S = H*P*H.transpose() +
|
||||
Feature::observation_noise*MatrixXd::Identity(
|
||||
H_thin.rows(), H_thin.rows());
|
||||
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
|
||||
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
|
||||
H.rows(), H.rows());
|
||||
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
|
||||
MatrixXd K_transpose = S.ldlt().solve(H*P);
|
||||
MatrixXd K = K_transpose.transpose();
|
||||
|
||||
// Compute the error of the state.
|
||||
VectorXd delta_x = K * r_thin;
|
||||
VectorXd delta_x = K * r;
|
||||
|
||||
// Update the IMU state.
|
||||
const VectorXd& delta_x_imu = delta_x.head<21>();
|
||||
@ -1722,7 +1736,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
auto cam_state_iter = state_server.cam_states.begin();
|
||||
for (int i = 0; i < state_server.cam_states.size();
|
||||
++i, ++cam_state_iter) {
|
||||
const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6);
|
||||
const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize);
|
||||
const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
|
||||
cam_state_iter->second.orientation = quaternionMultiplication(
|
||||
dq_cam, cam_state_iter->second.orientation);
|
||||
@ -1730,7 +1744,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
}
|
||||
|
||||
// Update state covariance.
|
||||
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
|
||||
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H;
|
||||
//state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
|
||||
// K*K.transpose()*Feature::observation_noise;
|
||||
state_server.state_cov = I_KH*state_server.state_cov;
|
||||
@ -1744,7 +1758,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
}
|
||||
|
||||
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
|
||||
|
||||
return true;
|
||||
MatrixXd P1 = H * state_server.state_cov * H.transpose();
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user