Skip to content

J
jfx_kalman_filter_src
Commit f5abb10c authored by oscar's avatar oscar
Browse files
Options

提交更新

parent 137d07d0
Hide whitespace changes
Inline Side-by-side
Showing with 24 additions and 15 deletions
TrackingRosEx.cpp
View file @ f5abb10c
......@@ -136,24 +136,33 @@ void get_camera_tolidar_loc(float u, float v, float& x, float& y)
Eigen::Vector3d loc_camera;
loc_camera << outputUndistortedPoints[0].x, outputUndistortedPoints[0].y, 1;
Eigen::Matrix<double, 3, 3> cam_trans;
cam_trans << -6.60431137e-02, 1.93080567e+00, -8.78993882e+02,//camera to point cloud transfrom
3.81973306e-02, -1.21347206e+00, 5.53062060e+02,
7.08859171e-05, -2.17867524e-03, 1.00000000e+00;
cam_trans << -1.29665805e-02, 9.37907698e-02, -6.31390808e+02,
8.18329480e-02, -1.38701295e-02, 4.67566705e+01,
-1.22717262e-03, -1.27333545e-02, 1.00000000e+00;
auto trans_pos = cam_trans * loc_camera;
double h = trans_pos[2];
auto trans_pos_ = trans_pos / h;
Eigen::Matrix<double, 3, 3> R;
R << -0.602479577065, -0.797206640244, -0.038464091718,
0.796449661255, -0.603639006615, 0.035887073725,
-0.051827855408, -0.009013481438, 0.998615324497;
Eigen::Vector3d T;
T << -886.266723632813, 556.056396484375, -0.398632347584;
auto R_inv = R.inverse();
auto P_ = R_inv * (trans_pos_ - T);
x = P_[0] - 3.5;
y = P_[1] + 0.4;
x = trans_pos_[0];
y = trans_pos_[1];
//Eigen::Matrix<double, 3, 3> cam_trans;
//cam_trans << -6.60431137e-02, 1.93080567e+00, -8.78993882e+02,//camera to point cloud transfrom
// 3.81973306e-02, -1.21347206e+00, 5.53062060e+02,
// 7.08859171e-05, -2.17867524e-03, 1.00000000e+00;
//auto trans_pos = cam_trans * loc_camera;
//double h = trans_pos[2];
//auto trans_pos_ = trans_pos / h;
//Eigen::Matrix<double, 3, 3> R;
//R << -0.602479577065, -0.797206640244, -0.038464091718,
// 0.796449661255, -0.603639006615, 0.035887073725,
// -0.051827855408, -0.009013481438, 0.998615324497;
//Eigen::Vector3d T;
//T << -886.266723632813, 556.056396484375, -0.398632347584;
//auto R_inv = R.inverse();
//auto P_ = R_inv * (trans_pos_ - T);
//x = P_[0] - 3.5;
//y = P_[1] + 0.4;
}
void get_camera_right_tolidar_loc(float u, float v, float& x, float& y)
{
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment