Skip to content

Commit

Permalink
fix loop and optimization mutex bug
Browse files Browse the repository at this point in the history
  • Loading branch information
@weihaoysgs
weihaoysgs committed Apr 15, 2024
1 parent d709ec5 commit f37e3e3
Show file tree
Hide file tree
Showing 5 changed files with 107 additions and 81 deletions.
138 changes: 74 additions & 64 deletions geometry/motion_ba/motion_ba.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,53 +7,53 @@ bool tceresMotionOnlyBA(
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
const bool buse_robust, const bool bapply_l2_after_robust, const Eigen::Matrix3d &K,
std::vector<int> &voutliersidx) {
float fx = K(0, 0);
float fy = K(1, 1);
float cx = K(0, 2);
float cy = K(1, 2);
return tceresMotionOnlyBA(vunkps, vwpts, vscales, Twc, nmaxiter, chi2th, buse_robust,
bapply_l2_after_robust, fx, fy, cx, cy, voutliersidx);
std::vector<int> &voutliersidx ) {
float fx = K( 0, 0 );
float fy = K( 1, 1 );
float cx = K( 0, 2 );
float cy = K( 1, 2 );
return tceresMotionOnlyBA( vunkps, vwpts, vscales, Twc, nmaxiter, chi2th, buse_robust,
bapply_l2_after_robust, fx, fy, cx, cy, voutliersidx );
}

bool tceresMotionOnlyBA(
const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > &vunkps,
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
const bool buse_robust, const bool bapply_l2_after_robust, const float fx, const float fy,
const float cx, const float cy, std::vector<int> &voutliersidx) {
assert(vunkps.size() == vwpts.size());
const float cx, const float cy, std::vector<int> &voutliersidx ) {
assert( vunkps.size() == vwpts.size() );

tceres::Problem problem;

double chi2thrtsq = std::sqrt(chi2th);
double chi2thrtsq = std::sqrt( chi2th );

tceres::LossFunctionWrapper *loss_function;
loss_function =
new tceres::LossFunctionWrapper(new tceres::HuberLoss(chi2thrtsq), tceres::TAKE_OWNERSHIP);
loss_function = new tceres::LossFunctionWrapper( new tceres::HuberLoss( chi2thrtsq ),
tceres::TAKE_OWNERSHIP );

if (!buse_robust) {
loss_function->Reset(NULL, tceres::TAKE_OWNERSHIP);
if ( !buse_robust ) {
loss_function->Reset( NULL, tceres::TAKE_OWNERSHIP );
}

size_t nbkps = vunkps.size();

tceres::LocalParameterization *local_parameterization = new backend::SE3LeftParameterization();

backend::PoseParametersBlock posepar = backend::PoseParametersBlock(0, Twc);
backend::PoseParametersBlock posepar = backend::PoseParametersBlock( 0, Twc );

problem.AddParameterBlock(posepar.values(), 7, local_parameterization);
problem.AddParameterBlock( posepar.values(), 7, local_parameterization );

std::vector<backend::DirectLeftSE3::ReprojectionErrorSE3 *> verrors_;
std::vector<tceres::ResidualBlockId> vrids_;

for (size_t i = 0; i < nbkps; i++) {
for ( size_t i = 0; i < nbkps; i++ ) {
auto *f = new backend::DirectLeftSE3::ReprojectionErrorSE3(
vunkps[i].x(), vunkps[i].y(), fx, fy, cx, cy, vwpts.at(i), std::pow(2., vscales[i]));
tceres::ResidualBlockId rid = problem.AddResidualBlock(f, loss_function, posepar.values());
vunkps[i].x(), vunkps[i].y(), fx, fy, cx, cy, vwpts.at( i ), std::pow( 2., vscales[i] ) );
tceres::ResidualBlockId rid = problem.AddResidualBlock( f, loss_function, posepar.values() );

verrors_.push_back(f);
vrids_.push_back(rid);
verrors_.push_back( f );
vrids_.push_back( rid );
}

tceres::Solver::Options options;
Expand All @@ -68,29 +68,29 @@ bool tceresMotionOnlyBA(
options.minimizer_progress_to_stdout = false;

tceres::Solver::Summary summary;
tceres::Solve(options, &problem, &summary);
tceres::Solve( options, &problem, &summary );

size_t bad = 0;

for (size_t i = 0; i < nbkps; i++) {
auto err = verrors_.at(i);
if (err->chi2err_ > chi2th || !err->isdepthpositive_) {
if (bapply_l2_after_robust) {
auto rid = vrids_.at(i);
problem.RemoveResidualBlock(rid);
for ( size_t i = 0; i < nbkps; i++ ) {
auto err = verrors_.at( i );
if ( err->chi2err_ > chi2th || !err->isdepthpositive_ ) {
if ( bapply_l2_after_robust ) {
auto rid = vrids_.at( i );
problem.RemoveResidualBlock( rid );
}
voutliersidx.push_back(i);
voutliersidx.push_back( i );
bad++;
}
}

if (bad == nbkps) {
if ( bad == nbkps ) {
return false;
}

if (bapply_l2_after_robust && !voutliersidx.empty()) {
loss_function->Reset(nullptr, tceres::TAKE_OWNERSHIP);
tceres::Solve(options, &problem, &summary);
if ( bapply_l2_after_robust && !voutliersidx.empty() ) {
loss_function->Reset( nullptr, tceres::TAKE_OWNERSHIP );
tceres::Solve( options, &problem, &summary );
}

Twc = posepar.getPose();
Expand All @@ -102,95 +102,105 @@ bool opencvP3PRansac(
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &bvs,
const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
const int nmaxiter, const float errth, const float fx, const float fy, const bool boptimize,
Sophus::SE3d &Twc, std::vector<int> &voutliersidx) {
assert(bvs.size() == vwpts.size());
Sophus::SE3d &Twc, std::vector<int> &voutliersidx ) {
assert( bvs.size() == vwpts.size() );

size_t nb3dpts = bvs.size();

if (nb3dpts < 4) {
if ( nb3dpts < 4 ) {
return false;
}

voutliersidx.reserve(nb3dpts);
voutliersidx.reserve( nb3dpts );

std::vector<cv::Point2f> cvbvs;
cvbvs.reserve(nb3dpts);
cvbvs.reserve( nb3dpts );

std::vector<cv::Point3f> cvwpts;
cvwpts.reserve(nb3dpts);
cvwpts.reserve( nb3dpts );

for (size_t i = 0; i < nb3dpts; i++) {
cvbvs.push_back(cv::Point2f(bvs.at(i).x() / bvs.at(i).z(), bvs.at(i).y() / bvs.at(i).z()));
for ( size_t i = 0; i < nb3dpts; i++ ) {
cvbvs.push_back(
cv::Point2f( bvs.at( i ).x() / bvs.at( i ).z(), bvs.at( i ).y() / bvs.at( i ).z() ) );

cvwpts.push_back(cv::Point3f(vwpts.at(i).x(), vwpts.at(i).y(), vwpts.at(i).z()));
cvwpts.push_back( cv::Point3f( vwpts.at( i ).x(), vwpts.at( i ).y(), vwpts.at( i ).z() ) );
}

// Using homoegeneous pts here so no dist or calib.
cv::Mat D;
cv::Mat K = cv::Mat::eye(3, 3, CV_32F);
cv::Mat K = cv::Mat::eye( 3, 3, CV_32F );

cv::Mat tvec, rvec;
cv::Mat inliers;

bool use_extrinsic_guess = false;
float confidence = 0.99;

float focal = (fx + fy) / 2.;
float focal = ( fx + fy ) / 2.;

cv::solvePnPRansac(cvwpts, cvbvs, K, D, rvec, tvec, use_extrinsic_guess, nmaxiter, errth / focal,
confidence, inliers, cv::SOLVEPNP_ITERATIVE);
try {
cv::solvePnPRansac( cvwpts, cvbvs, K, D, rvec, tvec, use_extrinsic_guess, nmaxiter,
errth / focal, confidence, inliers, cv::SOLVEPNP_ITERATIVE );
}
catch ( cv::Exception &e ) {
LOG( WARNING ) << "Catching a cv exception: " << e.msg;
return false;
}

if (inliers.rows == 0) {
if ( inliers.rows == 0 ) {
return false;
}

int k = 0;
for (size_t i = 0; i < nb3dpts; i++) {
if (inliers.at<int>(k) == (int)i) {
for ( size_t i = 0; i < nb3dpts; i++ ) {
if ( inliers.at<int>( k ) == (int)i ) {
k++;
if (k == inliers.rows) {
if ( k == inliers.rows ) {
k = 0;
}
} else {
voutliersidx.push_back(i);
voutliersidx.push_back( i );
}
}

if (voutliersidx.size() >= nb3dpts - 5) {
if ( voutliersidx.size() >= nb3dpts - 5 ) {
return false;
}

if (boptimize) {
if ( boptimize ) {
use_extrinsic_guess = true;

// Filter outliers
std::vector<cv::Point2f> in_cvbvs;
in_cvbvs.reserve(inliers.rows);
in_cvbvs.reserve( inliers.rows );

std::vector<cv::Point3f> in_cvwpts;
in_cvwpts.reserve(inliers.rows);
in_cvwpts.reserve( inliers.rows );

for (int i = 0; i < inliers.rows; i++) {
in_cvbvs.push_back(cvbvs.at(inliers.at<int>(i)));
in_cvwpts.push_back(cvwpts.at(inliers.at<int>(i)));
for ( int i = 0; i < inliers.rows; i++ ) {
in_cvbvs.push_back( cvbvs.at( inliers.at<int>( i ) ) );
in_cvwpts.push_back( cvwpts.at( inliers.at<int>( i ) ) );
}

cv::solvePnP(in_cvwpts, in_cvbvs, K, D, rvec, tvec, use_extrinsic_guess,
cv::SOLVEPNP_ITERATIVE);
if ( in_cvbvs.size() < 6 ) {
return false;
}
cv::solvePnP( in_cvwpts, in_cvbvs, K, D, rvec, tvec, use_extrinsic_guess,
cv::SOLVEPNP_ITERATIVE );
}

// Store the resulting pose
cv::Mat cvR;
cv::Rodrigues(rvec, cvR);
cv::Rodrigues( rvec, cvR );

Eigen::Vector3d tcw;
Eigen::Matrix3d Rcw;

cv::cv2eigen(cvR, Rcw);
cv::cv2eigen(tvec, tcw);
cv::cv2eigen( cvR, Rcw );
cv::cv2eigen( tvec, tcw );

Twc.translation() = -1. * Rcw.transpose() * tcw;
Twc.setRotationMatrix(Rcw.transpose());
Twc.setRotationMatrix( Rcw.transpose() );

return true;
}
Expand Down
25 changes: 15 additions & 10 deletions vio_hw/params/kitti/kitti_00-02.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,20 @@ Camera:
right.K: [718.856, 718.856, 607.1928, 185.2157]
left.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
right.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
FPS: 20

ConfigFilePath:
vocabulary.path: "/home/weihao/workspace/vio-hello-world-impl/thirdparty/Vocabulary/ORBvoc.txt"
kf.traj.save.path: "/home/weihao/workspace/vio-hello-world-impl/vio_hw/output/kitti_out/kitti_00_kf.txt"
dfm.config.file.path: "/home/weihao/workspace/vio-hello-world-impl/dfm/params/splg_config.yaml"
bag.file.path: ""


FeatureAndTracker:
feature.Type: 0
tracker.Type: 0
feature.Type: "HARRIS" # ["ORB_CV","HARRIS","ORB","SUPER_POINT"]
tracker.Type: "OPTICAL_FLOW" # ["OPTICAL_FLOW", "LIGHT_GLUE"]
# common params
max.kps.num: 300
max.kps.num: 500
max.kps.distance: 20
use.clahe: 0
clahe.val: 3
Expand All @@ -44,10 +47,12 @@ FeatureAndTracker:

IMU:
topic: "/imu0"
acc_n: 0.1
acc_w: 0.2
gyr_n: 0.3
gyr_w: 0.4
acc_n: 2.0000e-1
acc_w: 3.0000e-03
gyr_n: 1.7e-4
gyr_w: 1.9393e-05
g_norm: 9.81
FPS: 200

SLAM:
stereo.mode: 1
Expand All @@ -58,11 +63,11 @@ SLAM:

LoopCloser:
use.loop: 1
loop.threshold: 0.6
loop.threshold: 0.1

Backend:
use.backend: 0
opt.window.size: 15
use.backend: 1
opt.window.size: 10

# Camera Extrinsic parameters T_b_ci ( v_b = T_b_ci * v_ci )
Extrinsic:
Expand Down
16 changes: 14 additions & 2 deletions vio_hw/src/loop_closer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,10 @@ void LoopCloser::run() {

std::pair<int, float> candidate_kf_id_score = DetectLoop( cv_descs );

if ( candidate_kf_id_score.second > 0.10 ) {
LOG( INFO ) << "Loop Score: " << candidate_kf_id_score.second;
if ( candidate_kf_id_score.second > param_->loop_setting_.loop_threshold_ ) {
LOG( INFO ) << tceres::internal::StringPrintf( "Loop Score: %.3f, Loop Threshold [%.3f]",
candidate_kf_id_score.second,
param_->loop_setting_.loop_threshold_ );
} else {
continue;
}
Expand Down Expand Up @@ -108,13 +110,23 @@ void LoopCloser::ProcessLoopCandidate( int kf_loop_id ) {
if ( num_good_kps >= 30 ) {
double lc_pose_err = ( new_kf_->GetTcw() * Twc ).log().norm();

double min_lc_pose_err = 1.0;
if ( std::abs( lc_pose_err ) < min_lc_pose_err ) {
LOG( WARNING ) << tceres::internal::StringPrintf(
"LoopCloser Error %.5f too small, less than [%.3f] no need to carry out pose graph "
"optimization",
lc_pose_err, min_lc_pose_err );
return;
}

LOG( INFO ) << "[PoseGraph] >>> Closing a loop between : "
<< " KF #" << new_kf_->kfid_ << " (img #" << new_kf_->id_ << ") and KF #"
<< lc_kf->kfid_ << " (img #" << lc_kf->id_ << " ), lc pose err: " << lc_pose_err;

LOG( INFO ) << "loop kf pos: " << lc_kf->GetTwc().translation().transpose() << "; "
<< "new kf pos: " << new_kf_->GetTwc().translation().transpose() << "; "
<< "correct pos: " << Twc.translation().transpose();
std::unique_lock<std::mutex> lock2( map_manager_->optim_mutex_ );
optimization_->LocalPoseGraph( *new_kf_, lc_kf->kfid_, Twc );
}
}
Expand Down
6 changes: 3 additions & 3 deletions vio_hw/src/map_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,9 +85,9 @@ void MapManager::ExtractKeypoints( const cv::Mat& im, const cv::Mat& im_raw ) {

cv::Mat mask = cv::Mat( im.rows, im.cols, CV_8UC1, cv::Scalar( 255 ) );
for ( auto& pt : kps ) {
// cv::circle( mask, pt.px_, param_->feat_tracker_setting_.max_feature_dis_, 0, -1 );
cv::rectangle( mask, pt.px_ - cv::Point2f( 10, 10 ), pt.px_ + cv::Point2f( 10, 10 ), 0,
cv::FILLED );
cv::circle( mask, pt.px_, param_->feat_tracker_setting_.max_feature_dis_, 0, -1 );
// cv::rectangle( mask, pt.px_ - cv::Point2f( 10, 10 ), pt.px_ + cv::Point2f( 10, 10 ), 0,
// cv::FILLED );
}

int num_need_detect = param_->feat_tracker_setting_.max_feature_num_ - kps.size();
Expand Down
3 changes: 1 addition & 2 deletions vio_hw/src/optimization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ bool Optimization::LocalPoseGraph( Frame &new_frame, int kf_loop_id, const Sophu
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> vwpt;
std::vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> vTwc;

std::lock_guard<std::mutex> lock(map_manager_->map_mutex_);
// get updated KFs / MPs
for ( const auto &kf_id_pkf : map_kfs ) {
int kf_id = kf_id_pkf.first;
Expand Down Expand Up @@ -125,8 +126,6 @@ bool Optimization::LocalPoseGraph( Frame &new_frame, int kf_loop_id, const Sophu
}
}

std::lock_guard<std::mutex> lck( map_manager_->map_mutex_ );

Sophus::SE3d init_Tcw = new_frame.GetTcw();

// Propagate corrections to youngest KFs / MPs
Expand Down

0 comments on commit f37e3e3

Please sign in to comment.