changed structure for image undistort into image_handler
This commit is contained in:
parent
ebc73c0c5e
commit
b3e86b3e64
@ -138,8 +138,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
const CamStateServer& cam_states) const;
|
||||
|
||||
|
||||
bool initializeAnchorUndistort(const CameraCalibration& cam, int N);
|
||||
|
||||
/*
|
||||
* @brief InitializeAnchor generates the NxN patch around the
|
||||
* feature in the Anchor image
|
||||
@ -174,13 +172,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
Eigen::Vector3d& in_p) const;
|
||||
|
||||
|
||||
cv::Point2f projectPositionToCameraUndistorted(
|
||||
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
|
||||
@ -757,7 +748,7 @@ bool Feature::VisualizePatch(
|
||||
std::vector<double> projectionPatch;
|
||||
for(auto point : anchorPatch_3d)
|
||||
{
|
||||
cv::Point2f p_in_c0 = projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point);
|
||||
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
|
||||
projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
|
||||
// visu - feature
|
||||
cv::Point xs(p_in_c0.x, p_in_c0.y);
|
||||
@ -964,35 +955,6 @@ cv::Point2f Feature::pixelDistanceAt(
|
||||
return distance;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::projectPositionToCameraUndistorted(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const
|
||||
{
|
||||
Eigen::Isometry3d T_c0_w;
|
||||
|
||||
cv::Point2f out_p;
|
||||
|
||||
// transfrom position to camera frame
|
||||
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);
|
||||
|
||||
cv::Point2f my_p(out_p.x * cam.intrinsics[0] + cam.intrinsics[2], out_p.y * cam.intrinsics[1] + cam.intrinsics[3]);
|
||||
|
||||
// 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);
|
||||
|
||||
return my_p;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::projectPositionToCamera(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
@ -1009,15 +971,17 @@ cv::Point2f Feature::projectPositionToCamera(
|
||||
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));
|
||||
cv::Point2f my_p;
|
||||
|
||||
// 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);
|
||||
|
||||
cv::Point2f my_p = image_handler::distortPoint(out_p,
|
||||
// test is prewarped or not
|
||||
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);
|
||||
@ -1068,121 +1032,64 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
|
||||
auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
|
||||
|
||||
//testing
|
||||
undist_anchor_center_pos = cv::Point2f(u,v);
|
||||
|
||||
//for NxN patch pixels around feature
|
||||
int count = 0;
|
||||
// check if image has been pre-undistorted
|
||||
if(cam.distortion_model.substr(0,3) == "pre-")
|
||||
{
|
||||
std::cout << "is a pre" << std::endl;
|
||||
//project onto pixel plane
|
||||
undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
|
||||
|
||||
// get feature in undistorted pixel space
|
||||
// this only reverts from 'pure' space into undistorted pixel space using camera matrix
|
||||
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs);
|
||||
// create vector of patch in pixel plane
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
|
||||
// create vector of patch in pixel plane
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
|
||||
|
||||
//project back into u,v
|
||||
for(int i = 0; i < N*N; i++)
|
||||
anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
// get feature in undistorted pixel space
|
||||
// this only reverts from 'pure' space into undistorted pixel space using camera matrix
|
||||
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs);
|
||||
// create vector of patch in pixel plane
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
|
||||
|
||||
|
||||
//create undistorted pure points
|
||||
image_handler::undistortPoints(anchorPatch_real,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs,
|
||||
anchorPatch_ideal);
|
||||
//create undistorted pure points
|
||||
image_handler::undistortPoints(anchorPatch_real,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs,
|
||||
anchorPatch_ideal);
|
||||
|
||||
}
|
||||
|
||||
// save anchor position for later visualisaztion
|
||||
anchor_center_pos = anchorPatch_real[(N*N-1)/2];
|
||||
|
||||
// save true pixel Patch position
|
||||
for(auto point : anchorPatch_real)
|
||||
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
|
||||
return false;
|
||||
|
||||
for(auto point : anchorPatch_real)
|
||||
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
|
||||
|
||||
// project patch pixel to 3D space in camera coordinate system
|
||||
for(auto point : anchorPatch_ideal)
|
||||
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
|
||||
|
||||
is_anchored = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
//@test center projection must always be initial feature projection
|
||||
bool Feature::initializeAnchorUndistort(const CameraCalibration& cam, int N)
|
||||
{
|
||||
|
||||
//initialize patch Size
|
||||
int n = (int)(N-1)/2;
|
||||
|
||||
auto anchor = observations.begin();
|
||||
if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
|
||||
return false;
|
||||
|
||||
cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat anchorImage_deeper;
|
||||
anchorImage.convertTo(anchorImage_deeper,CV_16S);
|
||||
//TODO remove this?
|
||||
|
||||
|
||||
cv::Mat hen = cv::Mat::zeros(cv::Size(3, 3), CV_16S);
|
||||
hen.at<int>(1,0) = 1;
|
||||
|
||||
//sobel test
|
||||
/*
|
||||
cv::Mat newhen;
|
||||
cv::Sobel(hen, newhen, -1, 1, 0, 3);
|
||||
std::cout << "newhen" << newhen << std::endl;
|
||||
*/
|
||||
|
||||
cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
|
||||
cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
|
||||
|
||||
xderImage/=4;
|
||||
yderImage/=4;
|
||||
|
||||
cv::convertScaleAbs(xderImage, abs_xderImage);
|
||||
cv::convertScaleAbs(yderImage, abs_yderImage);
|
||||
|
||||
cvWaitKey(0);
|
||||
|
||||
auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
|
||||
auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
|
||||
|
||||
//project onto pixel plane
|
||||
undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
|
||||
|
||||
// create vector of patch in pixel plane
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
|
||||
|
||||
//project back into u,v
|
||||
for(int i = 0; i < N*N; i++)
|
||||
anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
|
||||
|
||||
|
||||
// save anchor position for later visualisaztion
|
||||
anchor_center_pos = anchorPatch_real[(N*N-1)/2];
|
||||
|
||||
|
||||
// save true pixel Patch position
|
||||
for(auto point : anchorPatch_real)
|
||||
if(point.x - n < 0 || point.x + n >= cam.resolution(0)-1 || point.y - n < 0 || point.y + n >= cam.resolution(1)-1)
|
||||
return false;
|
||||
|
||||
|
||||
for(auto point : anchorPatch_real)
|
||||
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
|
||||
|
||||
|
||||
// project patch pixel to 3D space in camera coordinate system
|
||||
for(auto point : anchorPatch_ideal)
|
||||
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
|
||||
|
||||
is_anchored = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -19,6 +19,13 @@ namespace image_handler {
|
||||
cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
|
||||
cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
|
||||
|
||||
void undistortImage(
|
||||
cv::InputArray src,
|
||||
cv::OutputArray dst,
|
||||
const std::string& distortion_model,
|
||||
const cv::Vec4d& intrinsics,
|
||||
const cv::Vec4d& distortion_coeffs);
|
||||
|
||||
void undistortPoints(
|
||||
const std::vector<cv::Point2f>& pts_in,
|
||||
const cv::Vec4d& intrinsics,
|
||||
|
||||
@ -18,11 +18,11 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
<param name="PHOTOMETRIC" value="false"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="false"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="StreamPause" value="true"/>
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
|
||||
@ -24,6 +24,24 @@ cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsic
|
||||
return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]);
|
||||
}
|
||||
|
||||
void undistortImage(
|
||||
cv::InputArray src,
|
||||
cv::OutputArray dst,
|
||||
const std::string& distortion_model,
|
||||
const cv::Vec4d& intrinsics,
|
||||
const cv::Vec4d& distortion_coeffs)
|
||||
{
|
||||
|
||||
const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
|
||||
0.0, intrinsics[1], intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
|
||||
if (distortion_model == "pre-equidistant")
|
||||
cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
|
||||
else if (distortion_model == "equidistant")
|
||||
src.copyTo(dst);
|
||||
}
|
||||
|
||||
void undistortPoint(
|
||||
const cv::Point2f& pt_in,
|
||||
const cv::Vec4d& intrinsics,
|
||||
|
||||
@ -215,8 +215,8 @@ void ImageProcessor::stereoCallback(
|
||||
const sensor_msgs::ImageConstPtr& cam1_img) {
|
||||
|
||||
// stop playing bagfile if printing images
|
||||
if(STREAMPAUSE)
|
||||
nh.setParam("/play_bag_image", false);
|
||||
//if(STREAMPAUSE)
|
||||
// nh.setParam("/play_bag_image", false);
|
||||
|
||||
// Get the current image.
|
||||
cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
|
||||
@ -225,17 +225,13 @@ void ImageProcessor::stereoCallback(
|
||||
sensor_msgs::image_encodings::MONO8);
|
||||
|
||||
|
||||
const cv::Matx33d K0(cam0.intrinsics[0], 0.0, cam0.intrinsics[2],
|
||||
0.0, cam0.intrinsics[1], cam0.intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
const cv::Matx33d K1(cam1.intrinsics[0], 0.0, cam1.intrinsics[2],
|
||||
0.0, cam1.intrinsics[1], cam1.intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
cv::Mat undistortedCam0;
|
||||
cv::Mat undistortedCam1;
|
||||
|
||||
|
||||
cv::fisheye::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, K0, cam0.distortion_coeffs, K0);
|
||||
cv::fisheye::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, K1, cam1.distortion_coeffs, K1);
|
||||
|
||||
//image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
//image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
|
||||
// Build the image pyramids once since they're used at multiple places
|
||||
createImagePyramids();
|
||||
|
||||
@ -306,8 +302,8 @@ void ImageProcessor::stereoCallback(
|
||||
}
|
||||
|
||||
// stop playing bagfile if printing images
|
||||
if(STREAMPAUSE)
|
||||
nh.setParam("/play_bag_image", true);
|
||||
//if(STREAMPAUSE)
|
||||
// nh.setParam("/play_bag_image", true);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -450,7 +450,7 @@ void MsckfVio::imageCallback(
|
||||
double imu_processing_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
//cout << "1" << endl;
|
||||
cout << "1" << endl;
|
||||
// Augment the state vector.
|
||||
start_time = ros::Time::now();
|
||||
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
@ -554,17 +554,15 @@ void MsckfVio::manageMovingWindow(
|
||||
|
||||
|
||||
// undistort Images
|
||||
|
||||
const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2],
|
||||
0.0, cam0.intrinsics[1], cam0.intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
|
||||
cv::Mat undistortedCam0;
|
||||
cv::fisheye::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, K, cam0.distortion_coeffs, K);
|
||||
//cv::imshow("anchor", undistortedCam0);
|
||||
//cvWaitKey(0);
|
||||
cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
|
||||
cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
|
||||
//cv::Mat undistortedCam0;
|
||||
//cv::Mat undistortedCam1;
|
||||
|
||||
//image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
//image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
|
||||
//cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
|
||||
//cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone();
|
||||
|
||||
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
|
||||
while(cam0.moving_window.size() > 100)
|
||||
@ -1322,8 +1320,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
//cout << "____feature-measurement_____\n" << endl;
|
||||
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
|
||||
cv::Point2f p_in_c0 = feature.projectPositionToCameraUndistorted(cam_state, cam_state_id, cam0, point);
|
||||
cv::Point2f p_in_anchor = feature.projectPositionToCameraUndistorted(anchor_state, anchor_state_id, cam0, point);
|
||||
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);
|
||||
|
||||
if( p_in_c0.x < 0 || p_in_c0.x >= cam0.resolution(0) || p_in_c0.y < 0 || p_in_c0.y >= cam0.resolution(1) )
|
||||
{
|
||||
@ -1509,7 +1507,7 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
H_x = A_null_space.transpose() * H_xi;
|
||||
r = A_null_space.transpose() * r_i;
|
||||
|
||||
//cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r);
|
||||
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
@ -1556,6 +1554,13 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< r_i << endl;
|
||||
|
||||
|
||||
myfile << "# name: xres\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << xres.rows() << "\n"
|
||||
<< "# columns: " << xres.cols() << "\n"
|
||||
<< xres << endl;
|
||||
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
cvWaitKey(0);
|
||||
@ -1693,9 +1698,10 @@ void MsckfVio::featureJacobian(
|
||||
FullPivLU<MatrixXd> lu(H_fj.transpose());
|
||||
MatrixXd A = lu.kernel();
|
||||
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
|
||||
Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r);
|
||||
// stop playing bagfile if printing images
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
@ -1715,13 +1721,19 @@ void MsckfVio::featureJacobian(
|
||||
<< "# columns: " << H_fj.cols() << "\n"
|
||||
<< H_fj << endl;
|
||||
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r_j.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< r_j << endl;
|
||||
|
||||
|
||||
myfile << "# name: xres\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << xres.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< xres << endl;
|
||||
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED ORG -------- " << endl;
|
||||
}
|
||||
@ -2002,7 +2014,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
}
|
||||
if(!feature.is_anchored)
|
||||
{
|
||||
if(!feature.initializeAnchorUndistort(cam0, N))
|
||||
if(!feature.initializeAnchor(cam0, N))
|
||||
{
|
||||
invalid_feature_ids.push_back(feature.id);
|
||||
continue;
|
||||
@ -2049,20 +2061,20 @@ void MsckfVio::removeLostFeatures() {
|
||||
MatrixXd pH_xj;
|
||||
VectorXd pr_j;
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
|
||||
//PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
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();
|
||||
}
|
||||
|
||||
/*
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
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();
|
||||
}
|
||||
|
||||
*/
|
||||
// Put an upper bound on the row size of measurement Jacobian,
|
||||
// which helps guarantee the executation time.
|
||||
//if (stack_cntr > 1500) break;
|
||||
@ -2075,7 +2087,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
//photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
// Remove all processed features from the map.
|
||||
for (const auto& feature_id : processed_feature_ids)
|
||||
@ -2168,7 +2180,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
}
|
||||
if(!feature.is_anchored)
|
||||
{
|
||||
if(!feature.initializeAnchorUndistort(cam0, N))
|
||||
if(!feature.initializeAnchor(cam0, N))
|
||||
{
|
||||
feature.observations.erase(rm_cam_state_id);
|
||||
continue;
|
||||
@ -2205,7 +2217,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
for (const auto& cam_state : state_server.cam_states)
|
||||
involved_cam_state_ids.push_back(cam_state.first);
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
//PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
|
||||
@ -2214,11 +2226,13 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
stack_cntr += H_xj.rows();
|
||||
pruned_cntr++;
|
||||
}
|
||||
/*
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
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();
|
||||
}
|
||||
*/
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
@ -2231,7 +2245,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
// Perform measurement update.
|
||||
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
//photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
//reduction
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
@ -2320,7 +2334,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
}
|
||||
if(!feature.is_anchored)
|
||||
{
|
||||
if(!feature.initializeAnchorUndistort(cam0, N))
|
||||
if(!feature.initializeAnchor(cam0, N))
|
||||
{
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
@ -2358,7 +2372,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
if (involved_cam_state_ids.size() == 0) continue;
|
||||
|
||||
PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
//PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
|
||||
@ -2366,13 +2380,13 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
}
|
||||
|
||||
/*
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
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();
|
||||
}
|
||||
|
||||
*/
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
@ -2385,7 +2399,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
// Perform measurement update.
|
||||
measurementUpdate(H_x, r);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
//photometricMeasurementUpdate(pH_x, pr);
|
||||
|
||||
//reduction
|
||||
for (const auto& cam_id : rm_cam_state_ids) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user