raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added stereo camera residual and jacobi to twomsckf - works

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-03 16:11:23 +02:00
parent 6bcc72f826
commit 6ee756941c
4 changed files with 99 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/msckf_vio/cam_state.h
View File

@@ -18,6 +18,8 @@ namespace msckf_vio {
struct Frame{
cv::Mat image;
cv::Mat dximage;
cv::Mat dyimage;
double exposureTime_ms;
};

36
include/msckf_vio/feature.hpp
View File

@@ -184,9 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
const CameraCalibration& cam,
Eigen::Vector3d& in_p) const;
double CompleteCvKernel(
double CompleteCvKernel(
const cv::Point2f pose,
const cv::Mat& frame,
const StateIDType& cam_state_id,
CameraCalibration& cam,
std::string type) const;
double cvKernel(
@@ -495,40 +496,17 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
double Feature::CompleteCvKernel(
const cv::Point2f pose,
const cv::Mat& frame,
const StateIDType& cam_state_id,
CameraCalibration& cam,
std::string type) const
{
double delta = 0;
cv::Mat xder;
cv::Mat yder;
cv::Mat deeper_frame;
frame.convertTo(deeper_frame,CV_16S);
//TODO remove this?
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
/*
cv::Mat norm_abs_xderImage;
cv::Mat abs_xderImage2;
cv::convertScaleAbs(xder, abs_xderImage2);
cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::imshow("xder", norm_abs_xderImage);
cvWaitKey(0);
*/
if(type == "Sobel_x")
delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
delta = ((double)cam.moving_window.find(cam_state_id)->second.dximage.at<short>(pose.y, pose.x))/255.;
else if (type == "Sobel_y")
delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
delta = ((double)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
return delta;
}

4
launch/msckf_vio_tinytum.launch
View File

@@ -18,14 +18,14 @@
output="screen">
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="FILTER" value="1"/>
<param name="FILTER" value="2"/>
<!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/>
<param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/>
<param name="patch_size_n" value="1"/>
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>

146
src/msckf_vio.cpp
View File

@@ -544,13 +544,22 @@ void MsckfVio::manageMovingWindow(
image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
// save image information into moving window
cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
cv::Mat xder;
cv::Mat yder;
cv::Mat deeper_frame;
cam1_img_ptr->image.convertTo(deeper_frame,CV_16S);
cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
xder/=8.;
yder/=8.;
cam0.moving_window[state_server.imu_state.id].dximage = xder.clone();
cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone();
@@ -1310,96 +1319,98 @@ void MsckfVio::twodotMeasurementJacobian(
const CAMState& cam_state = state_server.cam_states[cam_state_id];
const Feature& feature = map_server[feature_id];
// Cam0 pose.
// Cam0 pose
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Vector3d& t_c0_w = cam_state.position;
//photometric observation
std::vector<double> photo_z;
// Cam1 pose
Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
// individual Jacobians
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
Matrix<double, 4, 3> dh_dC0pij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 3> dh_dC1pij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 3> dh_dGpij = Matrix<double, 4, 3>::Zero();
Matrix<double, 4, 6> dh_dXplj = Matrix<double, 4, 6>::Zero();
Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dC0pij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 3> dC1pij_dGpij = Matrix<double, 3, 3>::Zero();
Matrix<double, 3, 6> dC0pij_dXplj = Matrix<double, 3, 6>::Zero();
Matrix<double, 3, 6> dC1pij_dXplj = Matrix<double, 3, 6>::Zero();
// one line of the NxN Jacobians
Eigen::Matrix<double, 2, 1> H_rho;
Eigen::Matrix<double, 2, 6> H_plj;
Eigen::Matrix<double, 2, 6> H_pAj;
Eigen::Matrix<double, 4, 1> H_rho;
Eigen::Matrix<double, 4, 6> H_plj;
Eigen::Matrix<double, 4, 6> H_pAj;
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
int count = 0;
auto point = feature.anchorPatch_3d[0];
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
Eigen::Vector3d p_c1 = R_w_c1 * (point-t_c1_w);
// add jacobian
//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));
dh_dC0pij(0, 0) = 1. / p_c0(2);
dh_dC0pij(1, 1) = 1. / p_c0(2);
dh_dC0pij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
dh_dC0pij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
//dh / d{}^Cp_{ij}
dh_dC1pij(2, 0) = 1. / p_c1(2);
dh_dC1pij(3, 1) = 1. / p_c1(2);
dh_dC1pij(2, 2) = -(p_c1(0))/(p_c1(2)*p_c1(2));
dh_dC1pij(3, 2) = -(p_c1(1))/(p_c1(2)*p_c1(2));
//orientation takes camera frame to world frame
dh_dGpij = dh_dCpij * dCpij_dGpij;
dC0pij_dGpij = R_w_c0;
dC1pij_dGpij = R_c0_c1 * R_w_c0;
//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;
dC0pij_dXplj.leftCols(3) = skewSymmetric(p_c0);
dC0pij_dXplj.rightCols(3) = -R_w_c0;
//d{}^Gp_P{ij} / \rho_i
// 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_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho));
// alternative derivation towards feature
Matrix3d dCpc0_dpg = R_w_c0;
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),
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho),
feature.anchorPatch_ideal[(N*N-1)/2].y/(rho),
1/(rho)));
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
// Intermediate Jakobians
H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1
H_plj = dh_dXplj; // 2 x 6
H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6
H_rho = dh_dC0pij * dC0pij_dGpij * dGpj_drhoj + dh_dC1pij * dC1pij_dGpij * dGpj_drhoj; // 4 x 1
H_plj = dh_dC0pij * dC0pij_dXplj + dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6
H_pAj = dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6
// calculate residual
//observation
const Vector4d& total_z = feature.observations.find(cam_state_id)->second;
const Vector2d z = Vector2d(total_z[0], total_z[1]);
VectorXd r_i = VectorXd::Zero(2);
VectorXd r_i = VectorXd::Zero(4);
//calculate residual
r_i[0] = z[0] - p_c0(0)/p_c0(2);
r_i[1] = z[1] - p_c0(1)/p_c0(2);
r_i[0] = total_z[0] - p_c0(0)/p_c0(2);
r_i[1] = total_z[1] - p_c0(1)/p_c0(2);
r_i[2] = total_z[2] - p_c1(0)/p_c1(2);
r_i[3] = total_z[3] - p_c1(1)/p_c1(2);
MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
MatrixXd H_xl = MatrixXd::Zero(4, 21+state_server.cam_states.size()*7);
// set anchor Jakobi
// 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);
H_xl.block(0, 21+cam_state_cntr_anchor*7, 2, 6) = H_pAj;
H_xl.block(0, 21+cam_state_cntr_anchor*7, 4, 6) = H_pAj;
// set frame Jakobi
//get position of current frame in cam states
@@ -1407,7 +1418,7 @@ void MsckfVio::twodotMeasurementJacobian(
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, 2, 6) = H_plj;
H_xl.block(0, 21+cam_state_cntr*7, 4, 6) = H_plj;
H_x = H_xl;
H_y = H_rho;
@@ -1446,7 +1457,7 @@ void MsckfVio::twodotFeatureJacobian(
}
int jacobian_row_size = 0;
jacobian_row_size = 2 * valid_cam_state_ids.size();
jacobian_row_size = 4 * valid_cam_state_ids.size();
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
21+state_server.cam_states.size()*7);
@@ -1458,7 +1469,7 @@ void MsckfVio::twodotFeatureJacobian(
MatrixXd H_xl;
MatrixXd H_yl;
Eigen::VectorXd r_l = VectorXd::Zero(2);
Eigen::VectorXd r_l = VectorXd::Zero(4);
twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
auto cam_state_iter = state_server.cam_states.find(cam_id);
@@ -1468,13 +1479,14 @@ void MsckfVio::twodotFeatureJacobian(
// Stack the Jacobians.
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
r_i.segment(stack_cntr, 2) = r_l;
stack_cntr += 2;
r_i.segment(stack_cntr, 4) = r_l;
stack_cntr += 4;
}
// Project the residual and Jacobians onto the nullspace
// of H_yj.
// get Nullspace
FullPivLU<MatrixXd> lu(H_yi.transpose());
MatrixXd A_null_space = lu.kernel();
@@ -1518,10 +1530,10 @@ void MsckfVio::twodotFeatureJacobian(
std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true);
}
return;
}
bool MsckfVio::PhotometricPatchPointResidual(
const StateIDType& cam_state_id,
const Feature& feature,
@@ -1628,8 +1640,8 @@ bool MsckfVio::PhotometricPatchPointJacobian(
// calculate derivation for anchor frame, use position for derivation calculation
// frame derivative calculated convoluting with kernel [-1, 0, 1]
dx = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_x");
dy = feature.CompleteCvKernel(p_in_c0, frame, "Sobel_y");
dx = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_x");
dy = feature.CompleteCvKernel(p_in_c0, cam_state_id, cam0, "Sobel_y");
//cout << "dx: " << dx << " : " << feature.cvKernel(p_in_c0, "Sobel_x") << " : " << feature.Kernel(p_in_c0, frame, "Sobel_x") << endl;
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
@@ -1806,6 +1818,8 @@ bool MsckfVio::PhotometricFeatureJacobian(
const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r)
{
return false;
const auto& feature = map_server[feature_id];
// Check how many camera states in the provided camera
@@ -2164,7 +2178,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// 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;
// cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
if(FILTER != 0) return;
@@ -2247,8 +2261,12 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
if (H.rows() == 0 || r.rows() == 0)
{
cout << "zero" << endl;
return;
}
// Decompose the final Jacobian matrix to reduce computational
// complexity as in Equation (28), (29).
MatrixXd H_thin;
@@ -2298,6 +2316,8 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
// Update the IMU state.
if (FILTER != 2) return;
cout << "two: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
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]);
@@ -2403,7 +2423,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
VectorXd delta_x = K * r;
// Update the IMU state.
cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
if (FILTER != 1) return;
@@ -2484,7 +2504,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
return true;
//return true;
MatrixXd P1 = H * state_server.state_cov * H.transpose();
MatrixXd P2 = Feature::observation_noise *
@@ -2492,8 +2512,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
cout << "gate: " << dof << " " << gamma << " " <<
chi_squared_test_table[dof] << endl;
//cout << "gate: " << dof << " " << gamma << " " <<
//chi_squared_test_table[dof] << endl;
if (chi_squared_test_table[dof] == 0)
return false;
@@ -2552,7 +2572,7 @@ void MsckfVio::removeLostFeatures() {
}
pjacobian_row_size += N*N*feature.observations.size();
twojacobian_row_size += 2*feature.observations.size();
twojacobian_row_size += 4*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
processed_feature_ids.push_back(feature.id);
@@ -2604,6 +2624,7 @@ void MsckfVio::removeLostFeatures() {
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
{
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
//cout << "passed" << endl;
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();
@@ -2619,6 +2640,7 @@ void MsckfVio::removeLostFeatures() {
stack_cntr += H_xj.rows();
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
@@ -2749,7 +2771,7 @@ void MsckfVio::pruneLastCamStateBuffer()
pjacobian_row_size += N*N*feature.observations.size();
jacobian_row_size += 4*feature.observations.size() - 3;
twojacobian_row_size += 2*feature.observations.size();
twojacobian_row_size += 4*feature.observations.size();
}
@@ -2787,6 +2809,7 @@ void MsckfVio::pruneLastCamStateBuffer()
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)) {
//cout << "passed" << endl;
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();
@@ -2804,6 +2827,7 @@ void MsckfVio::pruneLastCamStateBuffer()
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();
@@ -2929,7 +2953,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
}
twojacobian_row_size += 2*involved_cam_state_ids.size();
twojacobian_row_size += 4*involved_cam_state_ids.size();
pjacobian_row_size += N*N*involved_cam_state_ids.size();
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
}
@@ -2969,6 +2993,7 @@ void MsckfVio::pruneCamStateBuffer() {
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())) {
//cout << "passed" << endl;
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();
@@ -2985,6 +3010,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
cout << "passed" << endl;
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
twostack_cntr += twoH_xj.rows();