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

removed visu as result works so to not clutter the output

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-19 13:30:30 +02:00
parent 6f16f1b566
commit 1949e4c43d
2 changed files with 27 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
include/msckf_vio/feature.hpp
View File

@@ -384,8 +384,10 @@ bool Feature::estimate_FrameIrradiance(
double b_A = 0;
double a_l =frameExposureTime_ms;
double b_l = 0;
printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
printf("exposure: %f, %f\n", a_A, a_l);
//printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
//printf("exposure: %f, %f\n", a_A, a_l);
for (double anchorPixel : anchorPatch)
{
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
@@ -400,26 +402,24 @@ bool Feature::FrameIrradiance(
CameraCalibration& cam0,
std::vector<float>& anchorPatch_measurement) const
{
// project every point in anchorPatch_3d.
// int count = 0;
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
// visu
/*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
cv::Mat dottedFrame(current_image.size(), CV_8UC3);
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
*/
// project every point in anchorPatch_3d.
for (auto point : anchorPatch_3d)
{
// testing
//if(cam_state_id == observations.begin()->first)
//if(count == 4)
//printf("\n\ncenter:\n");
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
//visualization of feature
cv::Point xs(p_in_c0.x, p_in_c0.y);
// visu
/*cv::Point xs(p_in_c0.x, p_in_c0.y);
cv::Point ys(p_in_c0.x, p_in_c0.y);
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
*/
float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
anchorPatch_measurement.push_back(irradiance);
@@ -432,11 +432,11 @@ bool Feature::FrameIrradiance(
//visu
//cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
if(cam0.featureVisu.empty())
/*if(cam0.featureVisu.empty())
cam0.featureVisu = dottedFrame.clone();
else
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
*/
}
@@ -547,10 +547,7 @@ bool Feature::initializeAnchor(
for(auto point : vec)
{
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
{
printf("no good\n");
return false;
}
}
for(auto point : vec)
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));

18
src/msckf_vio.cpp
View File

@@ -977,7 +977,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
printf("-----\n");
// visu
//printf("-----\n");
//estimate photometric measurement
std::vector<float> estimate_photo_z;
@@ -988,9 +989,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
for(int i = 0; i < photo_z.size(); i++)
printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
// visu
//for(int i = 0; i < photo_z.size(); i++)
// printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
photo_z.clear();
return;
@@ -1022,9 +1023,12 @@ void MsckfVio::PhotometricFeatureJacobian(
VectorXd r_j = VectorXd::Zero(jacobian_row_size);
int stack_cntr = 0;
printf("_____FEATURE:_____\n");
// visu
/*
printf("_____FEATURE:_____\n");
cam0.featureVisu.release();
*/
for (const auto& cam_id : valid_cam_state_ids) {
Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
@@ -1042,6 +1046,8 @@ void MsckfVio::PhotometricFeatureJacobian(
r_j.segment<4>(stack_cntr) = r_i;
stack_cntr += 4;
}
// visu
/*
if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
imshow("feature", cam0.featureVisu);
cvWaitKey(1);
@@ -1053,6 +1059,8 @@ void MsckfVio::PhotometricFeatureJacobian(
imwrite(ss.str(), cam0.featureVisu);
image_save_time = ros::Time::now();
}
*/
// Project the residual and Jacobians onto the nullspace
// of H_fj.
JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);