diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 26916a5..95d567e 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -18,7 +18,7 @@ camera_model: '$(arg camera_model)' camera_parameters: $(arg camera_params) detector_type: 'GFTT' - detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + detector_parameters: {maxCorners: 1000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 tracker_num_scales: 4 tracker_delta_pixel_error_threshold: 3.0 diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc index 67b022e..2176feb 100644 --- a/src/module/odometry_module.cc +++ b/src/module/odometry_module.cc @@ -7,13 +7,14 @@ namespace omni_slam namespace module { -OdometryModule::OdometryModule(std::unique_ptr &pnp) - : pnp_(std::move(pnp)) +OdometryModule::OdometryModule(std::unique_ptr &pnp, std::unique_ptr &bundle_adjuster) + : pnp_(std::move(pnp)), + bundleAdjuster_(std::move(bundle_adjuster)) { } -OdometryModule::OdometryModule(std::unique_ptr &&pnp) - : OdometryModule(pnp) +OdometryModule::OdometryModule(std::unique_ptr &&pnp, std::unique_ptr &&bundle_adjuster) + : OdometryModule(pnp, bundle_adjuster) { } @@ -22,6 +23,11 @@ void OdometryModule::Update(std::vector &landmarks, data::Frame pnp_->Compute(landmarks, frame); } +void OdometryModule::BundleAdjust(std::vector &landmarks) +{ + bundleAdjuster_->Optimize(landmarks); +} + OdometryModule::Stats& OdometryModule::GetStats() { return stats_; diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h index 65b05a8..93d7189 100644 --- a/src/module/odometry_module.h +++ b/src/module/odometry_module.h @@ -6,6 +6,7 @@ #include #include "odometry/pnp.h" +#include "optimization/bundle_adjuster.h" #include "data/landmark.h" #include @@ -25,15 +26,17 @@ class OdometryModule { }; - OdometryModule(std::unique_ptr &pnp); - OdometryModule(std::unique_ptr &&pnp); + OdometryModule(std::unique_ptr &pnp, std::unique_ptr &bundle_adjuster); + OdometryModule(std::unique_ptr &&pnp, std::unique_ptr &&bundle_adjuster); void Update(std::vector &landmarks, data::Frame &frame); + void BundleAdjust(std::vector &landmarks); Stats& GetStats(); private: std::shared_ptr pnp_; + std::shared_ptr bundleAdjuster_; Stats stats_; }; diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index 082cdc4..fe0e5cf 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -1,6 +1,11 @@ #include "pnp.h" #include "lambda_twist.h" +#include +#include "optimization/reprojection_error.h" +#include "camera/double_sphere.h" +#include "camera/perspective.h" + #include "util/tf_util.h" #include @@ -22,6 +27,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram std::vector xs; std::vector yns; std::vector ys; + std::vector features; for (const data::Landmark &landmark : landmarks) { if (landmark.IsObservedInFrame(frame.GetID())) @@ -43,10 +49,16 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram pix << feat->GetKeypoint().pt.x, feat->GetKeypoint().pt.y; yns.push_back(pix); ys.push_back(feat->GetBearing()); + features.push_back(feat); } } Matrix pose; int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose); + std::vector indices = GetInlierIndices(xs, yns, pose, frame.GetCameraModel()); + if (inliers > 3) + { + Refine(xs, features, indices, pose); + } frame.SetEstimatedInversePose(pose); return inliers; } @@ -74,7 +86,7 @@ int PNP::RANSAC(const std::vector &xs, const std::vector &ys continue; } - int inliers = CountInliers(xs, yns, iterPose, camera_model); + int inliers = GetInlierIndices(xs, yns, iterPose, camera_model).size(); if (inliers > maxInliers) { maxInliers = inliers; @@ -84,6 +96,55 @@ int PNP::RANSAC(const std::vector &xs, const std::vector &ys return maxInliers; } +bool PNP::Refine(const std::vector &xs, const std::vector &features, const std::vector indices, Matrix &pose) const +{ + ceres::Problem problem; + ceres::LossFunction *loss_function = nullptr; + std::vector landmarks; + landmarks.reserve(3 * indices.size()); + Quaterniond quat(pose.block<3, 3>(0, 0)); + quat.normalize(); + Vector3d t(pose.block<3, 1>(0, 3)); + std::vector quatData(quat.coeffs().data(), quat.coeffs().data() + 4); + std::vector tData(t.data(), t.data() + 3); + for (int i : indices) + { + const Vector3d &x = xs[i]; + landmarks.push_back(x(0)); + landmarks.push_back(x(1)); + landmarks.push_back(x(2)); + ceres::CostFunction *cost_function = nullptr; + if (features[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective) + { + cost_function = optimization::ReprojectionError::Create(*features[i]); + } + else if (features[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere) + { + cost_function = optimization::ReprojectionError::Create(*features[i]); + } + if (cost_function != nullptr) + { + problem.AddResidualBlock(cost_function, loss_function, &quatData[0], &tData[0], &landmarks[landmarks.size() - 3]); + problem.SetParameterBlockConstant(&landmarks[landmarks.size() - 3]); + } + } + ceres::Solver::Options options; + options.max_num_iterations = 10; + options.linear_solver_type = ceres::DENSE_SCHUR; + options.function_tolerance = 1e-6; + options.gradient_tolerance = 1e-6; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + if (!summary.IsSolutionUsable()) + { + return false; + } + const Quaterniond quatRes = Map(&quatData[0]); + const Vector3d tRes = Map(&tData[0]); + pose = util::TFUtil::QuaternionTranslationToPoseMatrix(quatRes, tRes); + return true; +} + double PNP::P4P(const std::vector &xs, const std::vector &ys, const std::vector &yns, std::vector indices, const camera::CameraModel<> &camera_model, Matrix &pose) const { std::vector Rs(4); @@ -135,9 +196,9 @@ double PNP::P4P(const std::vector &xs, const std::vector &ys return error; } -int PNP::CountInliers(const std::vector &xs, const std::vector &yns, const Matrix &pose, const camera::CameraModel<> &camera_model) const +std::vector PNP::GetInlierIndices(const std::vector &xs, const std::vector &yns, const Matrix &pose, const camera::CameraModel<> &camera_model) const { - int inliers = 0; + std::vector indices; double thresh = reprojThreshold_ * reprojThreshold_; for (int i = 0; i < xs.size(); i++) { @@ -147,9 +208,12 @@ int PNP::CountInliers(const std::vector &xs, const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const; + bool Refine(const std::vector &xs, const std::vector &features, const std::vector indices, Matrix &pose) const; double P4P(const std::vector &xs, const std::vector &ys, const std::vector &yns, std::vector indices, const camera::CameraModel<> &camera_model, Matrix &pose) const; - int CountInliers(const std::vector &xs, const std::vector &yns, const Matrix &pose, const camera::CameraModel<> &camera_model) const; + std::vector GetInlierIndices(const std::vector &xs, const std::vector &yns, const Matrix &pose, const camera::CameraModel<> &camera_model) const; int ransacIterations_; double reprojThreshold_; diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc index e210049..39c2506 100644 --- a/src/optimization/bundle_adjuster.cc +++ b/src/optimization/bundle_adjuster.cc @@ -24,7 +24,7 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) { std::vector landmarkEstimates; landmarkEstimates.reserve(3 * landmarks.size()); - std::map> framePoses; + std::map, std::vector>> framePoses; std::map estFrames; ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); for (const data::Landmark &landmark : landmarks) @@ -69,17 +69,29 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) { - framePoses[feature.GetFrame().GetID()] = std::vector(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12); - problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12); - problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()][0]); + const Matrix &pose = feature.GetFrame().GetInversePose(); + Quaterniond quat(pose.block<3, 3>(0, 0)); + quat.normalize(); + const Vector3d &t = pose.block<3, 1>(0, 3); + framePoses[feature.GetFrame().GetID()] = {std::vector(quat.coeffs().data(), quat.coeffs().data() + 4), std::vector(t.data(), t.data() + 3)}; + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].first[0], 4); + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].second[0], 3); + problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()].first[0]); + problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()].second[0]); } } else if (feature.GetFrame().HasEstimatedPose()) { if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) { - framePoses[feature.GetFrame().GetID()] = std::vector(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12); - problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12); + const Matrix &pose = feature.GetFrame().GetInversePose(); + Quaterniond quat(pose.block<3, 3>(0, 0)); + quat.normalize(); + const Vector3d &t = pose.block<3, 1>(0, 3); + framePoses[feature.GetFrame().GetID()] = {std::vector(quat.coeffs().data(), quat.coeffs().data() + 4), std::vector(t.data(), t.data() + 3)}; + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].first[0], 4); + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].second[0], 3); + problem_->SetParameterization(&framePoses[feature.GetFrame().GetID()].first[0], new ceres::EigenQuaternionParameterization()); estFrames[feature.GetFrame().GetID()] = const_cast(&feature.GetFrame()); } } @@ -98,11 +110,15 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } if (cost_function != nullptr) { - problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]); + problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()].first[0], &framePoses[feature.GetFrame().GetID()].second[0], &landmarkEstimates[landmarkEstimates.size() - 3]); } } } + if (solverOptions_.minimizer_progress_to_stdout) + { + std::cout << "Running bundle adjustment..." << std::endl; + } ceres::Solver::Summary summary; ceres::Solve(solverOptions_, problem_.get(), &summary); if (solverOptions_.minimizer_progress_to_stdout) @@ -139,7 +155,9 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } for (auto &frame : estFrames) { - const Matrix pose = Map>(&framePoses[frame.first][0]); + const Quaterniond quat = Map(&framePoses[frame.first].first[0]); + const Vector3d t = Map(&framePoses[frame.first].second[0]); + const Matrix pose = util::TFUtil::QuaternionTranslationToPoseMatrix(quat, t); frame.second->SetEstimatedInversePose(pose); } return true; diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h index 5ec3604..4cfc68c 100644 --- a/src/optimization/reprojection_error.h +++ b/src/optimization/reprojection_error.h @@ -20,11 +20,13 @@ class ReprojectionError } template - bool operator()(const T* const camera_pose, const T* const point, T *reproj_error) const + bool operator()(const T* const camera_orientation, const T* const camera_translation, const T* const point, T *reproj_error) const { Matrix reprojPoint; C camera(feature_.GetFrame().GetCameraModel()); - const Matrix pose = Map>(camera_pose); + const Quaternion orientation = Map>(camera_orientation); + const Matrix translation = Map>(camera_translation); + const Matrix pose = util::TFUtil::QuaternionTranslationToPoseMatrix(orientation, translation); const Matrix worldPt = Map>(point); const Matrix camPt = util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, worldPt)); camera.ProjectToImage(camPt, reprojPoint); @@ -35,7 +37,7 @@ class ReprojectionError static ceres::CostFunction* Create(const data::Feature &feature) { - return new ceres::AutoDiffCostFunction(new ReprojectionError(feature)); + return new ceres::AutoDiffCostFunction(new ReprojectionError(feature)); } private: diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index ba7a91d..0dc5b56 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -1,6 +1,7 @@ #include "odometry_eval.h" #include "odometry/pnp.h" +#include "optimization/bundle_adjuster.h" #include "module/tracking_module.h" #include "geometry_msgs/PoseStamped.h" @@ -26,8 +27,9 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle nhp_.param("bundle_adjustment_logging", logCeres, false); unique_ptr pnp(new odometry::PNP(iterations, reprojThresh)); + unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres)); - odometryModule_.reset(new module::OdometryModule(pnp)); + odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster)); } void OdometryEval::InitPublishers() @@ -50,6 +52,7 @@ void OdometryEval::ProcessFrame(unique_ptr &&frame) void OdometryEval::Finish() { + odometryModule_->BundleAdjust(trackingModule_->GetLandmarks()); } void OdometryEval::GetResultsData(std::map>> &data)