added scaling changes to dI/dh for pixel size
This commit is contained in:
parent
e788854fe8
commit
d9899227c2
@ -148,6 +148,14 @@ struct Feature {
|
||||
inline bool initializePosition(
|
||||
const CamStateServer& cam_states);
|
||||
|
||||
cv::Point2f pixelDistanceAt(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const;
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* @brief project PositionToCamera Takes a 3d position in a world frame
|
||||
* and projects it into the passed camera frame using pinhole projection
|
||||
@ -737,6 +745,37 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
|
||||
return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::pixelDistanceAt(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const
|
||||
{
|
||||
|
||||
cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
|
||||
|
||||
// create vector of patch in pixel plane
|
||||
std::vector<cv::Point2f> surroundingPoints;
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
|
||||
|
||||
std::vector<cv::Point2f> pure;
|
||||
image_handler::undistortPoints(surroundingPoints,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs,
|
||||
pure);
|
||||
|
||||
// returns the absolute pixel distance at pixels one metres away
|
||||
cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
||||
|
||||
cv::Point2f Feature::projectPositionToCamera(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
|
||||
@ -18,13 +18,13 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
<param name="PHOTOMETRIC" value="false"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" 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="3"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
|
||||
@ -1225,6 +1225,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
const CAMState& cam_state = state_server.cam_states[cam_state_id];
|
||||
const Feature& feature = map_server[feature_id];
|
||||
|
||||
const StateIDType anchor_state_id = feature.observations.begin()->first;
|
||||
const CAMState anchor_state = state_server.cam_states[anchor_state_id];
|
||||
|
||||
// Cam0 pose.
|
||||
Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
|
||||
const Vector3d& t_c0_w = cam_state.position;
|
||||
@ -1263,7 +1266,7 @@ void 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;
|
||||
|
||||
@ -1291,8 +1294,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
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);
|
||||
|
||||
//add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
@ -1300,13 +1305,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
//calculate photom. residual
|
||||
photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
|
||||
|
||||
// add jacobians
|
||||
//cout << "residual: " << photo_r.back() << endl;
|
||||
|
||||
// add jacobians
|
||||
cv::Point2f pixelDistance = feature.pixelDistanceAt(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.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;
|
||||
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/pixelDistance.x;
|
||||
dI_dhj(0, 1) = dy/pixelDistance.y;
|
||||
|
||||
gradientVector.x += dx;
|
||||
gradientVector.y += dy;
|
||||
@ -1332,8 +1341,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
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
|
||||
// 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));
|
||||
|
||||
@ -1352,6 +1362,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_pl.block<1, 6>(count, 0) = H_plj;
|
||||
H_pA.block<1, 6>(count, 0) = H_pAj;
|
||||
|
||||
//cout << "H_pl\n" << H_plj << endl;
|
||||
|
||||
//cout << "H_pA\n" << H_pAj << endl;
|
||||
|
||||
//cout << "H_rho\n" << H_rhoj << endl;
|
||||
count++;
|
||||
}
|
||||
|
||||
@ -1491,15 +1506,18 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
|
||||
MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
|
||||
*/
|
||||
|
||||
H_x = A_null_space.transpose() * H_xi;
|
||||
r = A_null_space.transpose() * r_i;
|
||||
/*
|
||||
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
*/
|
||||
}
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
std::cout << "resume playback" << std::endl;
|
||||
@ -1642,7 +1660,7 @@ void MsckfVio::featureJacobian(
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl;
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
return;
|
||||
@ -1866,7 +1884,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
else
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
@ -2028,7 +2046,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
else
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user