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

added structure for stereo photometry - diverging

Browse Source
This commit is contained in:
Raphael Maenle
2019-07-03 17:48:54 +02:00
parent 6ee756941c
commit 3873c978dd
5 changed files with 161 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
include/msckf_vio/feature.hpp
View File

@ -507,6 +507,7 @@ double Feature::CompleteCvKernel(
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)cam.moving_window.find(cam_state_id)->second.dyimage.at<short>(pose.y, pose.x))/255.;
return delta;
}
@ -990,28 +991,39 @@ cv::Point2f Feature::projectPositionToCamera(
cv::Point2f out_p;
cv::Point2f my_p;
// transfrom position to camera frame
// cam0 position
Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
const Eigen::Vector3d& t_c0_w = cam_state.position;
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
// if(cam_state_id == observations.begin()->first)
//printf("undist:\n \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
// project point according to model
if(cam.id == 0)
{
Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
}
// if camera is one, calcualte the cam1 position from cam0 position first
else if(cam.id == 1)
{
// cam1 position
Eigen::Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
Eigen::Matrix3d R_w_c1 = R_c0_c1 * R_w_c0;
Eigen::Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
Eigen::Vector3d p_c1 = R_w_c1 * (in_p-t_c1_w);
out_p = cv::Point2f(p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
}
// undistort point according to camera model
if (cam.distortion_model.substr(0,3) == "pre-")
my_p = cv::Point2f(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
else
my_p = image_handler::distortPoint(out_p,
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
// printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
// printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
// printf("Photo projection: %f, %f\n", my_p[0].x, my_p[0].y);
cam.intrinsics,
cam.distortion_model,
cam.distortion_coeffs);
return my_p;
}