added visualization with a ros flag, which shows feature with projection and residual (the features apparent movement)
This commit is contained in:
parent
750d8ecfb1
commit
cf40ce8afb
@ -171,8 +171,8 @@ struct Feature {
|
||||
bool VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0) const;
|
||||
|
||||
CameraCalibration& cam0,
|
||||
const std::vector<double> photo_r) const;
|
||||
/*
|
||||
* @brief projectPixelToPosition uses the calcualted pixels
|
||||
* of the anchor patch to generate 3D positions of all of em
|
||||
@ -204,6 +204,7 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
|
||||
// NxN Patch of Anchor Image
|
||||
std::vector<double> anchorPatch;
|
||||
std::vector<cv::Point2f> anchorPatch_ideal;
|
||||
std::vector<cv::Point2f> anchorPatch_real;
|
||||
|
||||
// Position of NxN Patch in 3D space
|
||||
std::vector<Eigen::Vector3d> anchorPatch_3d;
|
||||
@ -408,43 +409,104 @@ bool Feature::estimate_FrameIrradiance(
|
||||
bool Feature::VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0) const
|
||||
CameraCalibration& cam0,
|
||||
const std::vector<double> photo_r) const
|
||||
{
|
||||
|
||||
double rescale = 1;
|
||||
|
||||
//visu - anchor
|
||||
auto anchor = observations.begin();
|
||||
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
|
||||
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
|
||||
|
||||
for(auto point : anchorPatch_real)
|
||||
{
|
||||
// visu - feature
|
||||
cv::Point xs(point.x, point.y);
|
||||
cv::Point ys(point.x, point.y);
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
|
||||
}
|
||||
cam0.featureVisu = dottedFrame.clone();
|
||||
|
||||
// visu - feature
|
||||
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.
|
||||
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
|
||||
// set position in frame
|
||||
// save irradiance of projection
|
||||
std::vector<double> projectionPatch;
|
||||
for(auto point : anchorPatch_3d)
|
||||
{
|
||||
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);
|
||||
cv::Point ys(p_in_c0.x, p_in_c0.y);
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
|
||||
}
|
||||
// testing
|
||||
//if(cam_state_id == observations.begin()->first)
|
||||
//if(count++ == 4)
|
||||
//printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
|
||||
|
||||
// visu - feature
|
||||
cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
|
||||
if(cam0.featureVisu.empty())
|
||||
cam0.featureVisu = dottedFrame.clone();
|
||||
else
|
||||
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
|
||||
|
||||
// irradiance grid anchor
|
||||
int N = sqrt(anchorPatch_3d.size());
|
||||
int scale = 20;
|
||||
cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
|
||||
cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale);
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N; j++)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(10+scale*(i+1), 10+scale*j),
|
||||
cv::Point(10+scale*i, 10+scale*(j+1)),
|
||||
cv::Scalar(anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255),
|
||||
CV_FILLED);
|
||||
|
||||
// irradiance grid projection
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N ; j++)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(10+scale*(i+1), 20+scale*(N+j)),
|
||||
cv::Point(10+scale*(i), 20+scale*(N+j+1)),
|
||||
cv::Scalar(projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255),
|
||||
CV_FILLED);
|
||||
|
||||
// residual grid projection, positive - red, negative - blue colored
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N; j++)
|
||||
if(photo_r[i*N+j]>0)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)),
|
||||
cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)),
|
||||
cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255),
|
||||
CV_FILLED);
|
||||
else
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)),
|
||||
cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)),
|
||||
cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255),
|
||||
CV_FILLED);
|
||||
|
||||
cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
|
||||
|
||||
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
|
||||
|
||||
// create line?
|
||||
|
||||
//save image
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
|
||||
cv::imwrite(ss.str(), cam0.featureVisu);
|
||||
|
||||
//cv::imshow("patch", cam0.featureVisu);
|
||||
//cvWaitKey(1);
|
||||
}
|
||||
|
||||
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
|
||||
{
|
||||
|
||||
return ((float)image.at<uint8_t>(pose.x, pose.y))/256;
|
||||
return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::projectPositionToCamera(
|
||||
@ -514,16 +576,16 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
int count = 0;
|
||||
|
||||
// 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,
|
||||
0);
|
||||
// create vector of patch in pixel plane
|
||||
std::vector<cv::Point2f> und_pix_v;
|
||||
std::vector<cv::Point2f>und_pix_v;
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run));
|
||||
|
||||
und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
|
||||
|
||||
|
||||
//create undistorted pure points
|
||||
@ -533,23 +595,24 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
cam.distortion_model,
|
||||
0,
|
||||
und_v);
|
||||
|
||||
//create distorted pixel points
|
||||
std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
|
||||
anchorPatch_real = image_handler::distortPoints(und_v,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs);
|
||||
|
||||
|
||||
// save anchor position for later visualisaztion
|
||||
anchor_center_pos = vec[4];
|
||||
anchor_center_pos = anchorPatch_real[(N*N-1)/2];
|
||||
|
||||
// save true pixel Patch position
|
||||
for(auto point : vec)
|
||||
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 : vec)
|
||||
for(auto point : anchorPatch_real)
|
||||
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
|
||||
|
||||
// project patch pixel to 3D space
|
||||
|
||||
@ -202,7 +202,9 @@ class MsckfVio {
|
||||
void onlineReset();
|
||||
|
||||
// Photometry flag
|
||||
// visualization flag
|
||||
bool PHOTOMETRIC;
|
||||
bool PRINTIMAGES;
|
||||
|
||||
bool nan_flag;
|
||||
|
||||
|
||||
@ -19,8 +19,9 @@
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
<param name="patch_size_n" value="7"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
|
||||
@ -63,7 +63,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
|
||||
bool MsckfVio::loadParameters() {
|
||||
//Photometry Flag
|
||||
nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
|
||||
|
||||
nh.param<bool>("PrintImages", PRINTIMAGES, false);
|
||||
|
||||
// Frame id
|
||||
nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
|
||||
@ -1027,11 +1027,11 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
//add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
//add jacobian
|
||||
// add jacobian
|
||||
|
||||
// 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);
|
||||
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;
|
||||
|
||||
@ -1043,7 +1043,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
//dh / d X_{pl}
|
||||
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * skewSymmetric(point);
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation).transpose();
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * -quaternionToRotation(cam_state.orientation);//.transpose();
|
||||
|
||||
//d{}^Gp_P{ij} / \rho_i
|
||||
double rho = feature.anchor_rho;
|
||||
@ -1074,6 +1074,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
std::vector<double> estimate_photo_z;
|
||||
IlluminationParameter estimated_illumination;
|
||||
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
|
||||
|
||||
for (auto& estimate_irradiance_j : estimate_irradiance)
|
||||
estimate_photo_z.push_back (estimate_irradiance_j *
|
||||
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
|
||||
@ -1121,6 +1122,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
for(auto data : photo_r)
|
||||
r[count++] = data;
|
||||
|
||||
if(PRINTIMAGES)
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -1151,15 +1155,8 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
|
||||
// visu - residual
|
||||
//printf("_____FEATURE:_____\n");
|
||||
// visu - feature
|
||||
//cam0.featureVisu.release();
|
||||
|
||||
for (const auto& cam_id : valid_cam_state_ids) {
|
||||
|
||||
//Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
|
||||
//Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
|
||||
MatrixXd H_xl;
|
||||
MatrixXd H_yl;
|
||||
Eigen::VectorXd r_l = VectorXd::Zero(N*N);
|
||||
@ -1176,27 +1173,11 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
r_i.segment(stack_cntr, N*N) = r_l;
|
||||
stack_cntr += N*N;
|
||||
}
|
||||
// visu - feature
|
||||
/*
|
||||
if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
|
||||
imshow("feature", cam0.featureVisu);
|
||||
cvWaitKey(1);
|
||||
|
||||
if((ros::Time::now() - image_save_time).toSec() > 1)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
|
||||
imwrite(ss.str(), cam0.featureVisu);
|
||||
image_save_time = ros::Time::now();
|
||||
}
|
||||
*/
|
||||
|
||||
// Project the residual and Jacobians onto the nullspace
|
||||
// of H_yj.
|
||||
|
||||
// get Nullspace
|
||||
|
||||
|
||||
JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
|
||||
int sv_size = 0;
|
||||
Eigen::VectorXd singularValues = svd_helper.singularValues();
|
||||
@ -1339,8 +1320,6 @@ void MsckfVio::featureJacobian(
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
|
||||
cout << "A: \n" << A.transpose() << endl;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -1348,8 +1327,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
|
||||
if (H.rows() == 0 || r.rows() == 0) return;
|
||||
|
||||
|
||||
cout << "Updating...";
|
||||
// Decompose the final Jacobian matrix to reduce computational
|
||||
// complexity as in Equation (28), (29).
|
||||
MatrixXd H_thin;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user