public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

best config till 20.09.21 the drift on x has been removed

Browse Source
This commit is contained in:
gfuse
2021-09-20 10:04:33 +02:00
parent e8e35fe23e
commit 31761fe5de
7 changed files with 10311 additions and 1235 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+9427 -4
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+836 -1187
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/homography.h
+6 -2
View File
@@ -45,6 +45,8 @@ public:
Homography (const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts1,
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts2,
double _error_multiplier2,
double _thresh_in_px);
@@ -69,8 +71,10 @@ public:
double thresh;
double error_multiplier2;
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on unit plane
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on unit plane
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on px
const vector<Vector3d, aligned_allocator<Vector3d> >& pfts_c1; //!< Features on first image in world
const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on px
const vector<Vector3d, aligned_allocator<Vector3d> >& pfts_c2; //!< Features on second image on unit plane
vector<bool> inliers;
SE3 T_c2_from_c1; //!< Relative translation and rotation of two images
Matrix3d H_c2_from_c1; //!< Homography
GPU_version/vio/include/vio/initialization.h
+19 -21
View File
@@ -79,33 +79,31 @@ protected:
f_cur.at(ftr.index) = frame->c2f(px_cur_.at(ftr.index).x,px_cur_.at(ftr.index).y);
f_ref.at(ftr.index) = ftr.item->f;
}
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
vk::Homography Homography(uv_ref, uv_cur,f_ref,f_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
vector<int> outliers;
vector<int> outliers_hom, inlier_hom;
vector<int> outliers_ekf,inlier_ekf;
assert(ukf_!= nullptr);
auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inlier_hom, outliers_hom);
auto res=ukf_->get_location();
size_t in=0;
for(auto&& i:Homography.inliers)if(i)++in;
#if VIO_DEBUG
fprintf(log_,"[%s] homography: x=%f y=%f z=%f roll=%f pitch=%f yaw=%f \t ekf: x=%f z=%f pitch=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().y(),Homography.T_c2_from_c1.translation().z(),
euler(0),euler(1),euler(2)
,res.second.se2().translation().x(),res.second.se2().translation().y(),res.second.pitch());
#endif
if(in>20)
res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
vk::computeInliers(f_cur, f_ref,
res.second.se3().rotation_matrix(), res.second.se3().translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
#if VIO_DEBUG
fprintf(log_,"[%s] homography after fuse: x=%f y=%f z=%f pitch=%f\t number of outlier: %d\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
res.second.se3().translation().x(),res.second.se3().translation().y(),res.second.se3().translation().z(),res.second.pitch(),outliers.size());
#endif
T_cur_from_ref=res.second;
xyz_in_cur, inlier_ekf, outliers_ekf);
if(inlier_ekf.size()>inlier_hom.size()){
T_cur_from_ref=res.second;
inliers=inlier_ekf;
}else if((Homography.T_c2_from_c1.translation()-res.second.se3().translation()).norm()<0.1 && inlier_ekf.size()<inlier_hom.size()){
auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
T_cur_from_ref=res.second;
inliers=inlier_hom;
}
}
};
GPU_version/vio/include/vio/ukf.h
+3 -3
View File
@@ -129,12 +129,12 @@ public:
//Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(10.0*x,-10.0*z,-1.0*pitch);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
filter_->correct(x,-1.0*z,-1.0*pitch);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
boost::unique_lock<boost::mutex> lock(ekf_mut_);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
}
bool start_=false;
private:
GPU_version/vio/src/homography.cpp
+19 -17
View File
@@ -21,12 +21,16 @@ namespace vk {
Homography::
Homography(const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts1,
const vector<Vector3d, aligned_allocator<Vector3d> >& _pfts2,
double _error_multiplier2,
double _thresh_in_px) :
thresh(_thresh_in_px),
error_multiplier2(_error_multiplier2),
fts_c1(_fts1),
fts_c2(_fts2)
fts_c2(_fts2),
pfts_c1(_pfts1),
pfts_c2(_pfts2)
{
}
@@ -49,7 +53,7 @@ namespace vk {
// TODO: replace this function to remove dependency from opencv!
// cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2./error_multiplier2);
cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC, 2./error_multiplier2);//2./error_multiplier2
cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC,2./error_multiplier2 );//2./error_multiplier2
H_c2_from_c1(0,0) = cvH.at<double>(0,0);
H_c2_from_c1(0,1) = cvH.at<double>(0,1);
H_c2_from_c1(0,2) = cvH.at<double>(0,2);
@@ -68,10 +72,8 @@ namespace vk {
size_t n_inliers = 0;
for(size_t i=0; i<fts_c1.size(); i++)
{
Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
Vector2d e = fts_c2[i] - projected;
double e_px = error_multiplier2 * e.norm();
inliers[i] = (e_px < thresh);
Vector3d e = pfts_c2[i] - (H_c2_from_c1 * pfts_c1[i]);
inliers[i] = (e.norm() < 15.0);
n_inliers += inliers[i];
}
return n_inliers;
@@ -85,7 +87,7 @@ namespace vk {
bool res = decompose();
if(!res)
return false;
computeMatchesInliers();
//computeMatchesInliers();
findBestDecomposition();
Eigen::Quaternion<double> q(decompositions.front().T.rotation_matrix());
q.vec() *= 1/q.norm();
@@ -155,7 +157,7 @@ namespace vk {
decomp.t[1] = 0.0;
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[0] = 0.0;//x1_PM * e1[signs];
np[1] = 0.0;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
@@ -171,9 +173,9 @@ namespace vk {
decomp.R = -1 * Matrix3d::Identity();
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
/*
* correct the rotation matrix by Majid
*/
// correct the rotation matrix by Majid
decomp.R(0,0) = dCosPhi;
decomp.R(0,2) = dSinPhi;
decomp.R(2,0) = dSinPhi;
@@ -184,7 +186,7 @@ namespace vk {
decomp.t[1] = 0.0;
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[0] = 0.0;//x1_PM * e1[signs];
np[1] = 0.0;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
@@ -217,8 +219,8 @@ namespace vk {
size_t nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
/* if(!inliers[m])
continue;*/
const Vector2d& v2 = fts_c1[m];
double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
if(dVisibilityTest > 0.0)
@@ -236,9 +238,9 @@ namespace vk {
int nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
Vector3d v3 = unproject2d(fts_c1[m]);
/* if(!inliers[m])
continue;*/
Vector3d v3 = pfts_c1[m];
double dVisibilityTest = v3.dot(decom.n) / decom.d;
if(dVisibilityTest > 0.0)
nPositive++;
GPU_version/vio/src/vo_node.cpp
+1 -1
View File
@@ -159,7 +159,7 @@ void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
#if VIO_DEBUG
auto odom=vo_->ukfPtr_.get_location();
fprintf(vo_->log_,"[%s] Odometry x=%f, y=%f, theta=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
10.0*odom.second.se2().translation()(0),10.0*odom.second.se2().translation()(1),
odom.second.se2().translation()(0),odom.second.se2().translation()(1),
odom.second.pitch());
#endif
}