From 0c83b818cc581767d5ec37f1f6a3f34081197af3 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Wed, 11 Sep 2019 13:38:12 -0400 Subject: [PATCH] start bundle adjustment not working --- CMakeLists.txt | 11 +- launch/reconstruction_eval.launch | 2 + src/camera/camera_model.h | 7 +- src/camera/double_sphere.cc | 121 ---------------------- src/camera/double_sphere.h | 135 +++++++++++++++++++++--- src/camera/perspective.cc | 46 --------- src/camera/perspective.h | 56 ++++++++-- src/data/frame.cc | 19 ++-- src/data/frame.h | 15 +-- src/data/landmark.cc | 2 +- src/data/landmark.h | 2 +- src/module/reconstruction_module.cc | 22 +++- src/module/reconstruction_module.h | 7 +- src/optimization/bundle_adjuster.cc | 141 ++++++++++++++++++++++++++ src/optimization/bundle_adjuster.h | 26 +++++ src/optimization/reprojection_error.h | 55 ++++++++++ src/ros/eval_base.cc | 14 ++- src/ros/eval_base.h | 3 +- src/ros/reconstruction_eval.cc | 22 +++- src/ros/reconstruction_eval.h | 1 + src/util/math_util.cc | 37 ------- src/util/math_util.h | 28 ++++- src/util/tf_util.cc | 57 ----------- src/util/tf_util.h | 55 ++++++++-- 24 files changed, 559 insertions(+), 325 deletions(-) delete mode 100644 src/camera/double_sphere.cc delete mode 100644 src/camera/perspective.cc create mode 100644 src/optimization/bundle_adjuster.cc create mode 100644 src/optimization/bundle_adjuster.h create mode 100644 src/optimization/reprojection_error.h delete mode 100644 src/util/math_util.cc delete mode 100644 src/util/tf_util.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index ba7d0c0..4faee83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,9 @@ include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) +find_package(Ceres REQUIRED) +include_directories(${Ceres_INCLUDE_DIRS}) + find_package(HDF5 COMPONENTS C CXX HL REQUIRED) include_directories(${HDF5_INCLUDE_DIR}) link_directories(${HDF5_LIBRARY_DIRS}) @@ -59,14 +62,11 @@ add_library(omni_slam_eval_lib src/data/frame.cc src/data/feature.cc src/data/landmark.cc - src/camera/perspective.cc - src/camera/double_sphere.cc src/feature/tracker.cc src/feature/detector.cc src/feature/matcher.cc src/reconstruction/triangulator.cc - src/util/tf_util.cc - src/util/math_util.cc + src/optimization/bundle_adjuster.cc src/util/hdf_file.cc ) @@ -95,6 +95,7 @@ target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} + ceres ) target_link_libraries(omni_slam_matching_eval_node PUBLIC omni_slam_eval_lib @@ -103,6 +104,7 @@ target_link_libraries(omni_slam_matching_eval_node PUBLIC omni_slam_eval_lib ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} + ceres ) target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_lib @@ -111,6 +113,7 @@ target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_l ${HDF5_CXX_LIBRARIES} ${OpenMP_CXX_FLAGS} ${PCL_LIBRARIES} + ceres ) target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index be1f031..7827c91 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -24,6 +24,8 @@ min_features_per_region: 10 max_reprojection_error: 0.0 min_triangulation_angle: 3.0 + bundle_adjustment_max_iterations: 1000 + bundle_adjustment_logging: true diff --git a/src/camera/camera_model.h b/src/camera/camera_model.h index f744355..8fc1650 100644 --- a/src/camera/camera_model.h +++ b/src/camera/camera_model.h @@ -11,6 +11,7 @@ namespace omni_slam namespace camera { +template class CameraModel { public: @@ -18,9 +19,9 @@ class CameraModel { name_ = name; } - virtual bool ProjectToImage(const Vector3d &bearing, Vector2d &pixel) const = 0; - virtual bool UnprojectToBearing(const Vector2d &pixel, Vector3d &bearing) const = 0; - virtual double GetFOV() = 0; + virtual bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const = 0; + virtual bool UnprojectToBearing(const Matrix &pixel, Matrix &bearing) const = 0; + virtual T GetFOV() = 0; private: std::string name_; diff --git a/src/camera/double_sphere.cc b/src/camera/double_sphere.cc deleted file mode 100644 index 2410d23..0000000 --- a/src/camera/double_sphere.cc +++ /dev/null @@ -1,121 +0,0 @@ -#include "double_sphere.h" -#include -#include - -namespace omni_slam -{ -namespace camera -{ - -DoubleSphere::DoubleSphere(const double fx, const double fy, const double cx, const double cy, const double chi, const double alpha) - : CameraModel("DoubleSphere"), - fx_(fx), - fy_(fy), - cx_(cx), - cy_(cy), - chi_(chi), - alpha_(alpha) -{ - cameraMat_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1; - fov_ = GetFOV(); - double theta = M_PI / 2 - fov_ / 2; - sinTheta_ = sin(theta); -} - -bool DoubleSphere::ProjectToImage(const Vector3d &bearing, Vector2d &pixel) const -{ - double x = bearing(0); - double y = bearing(1); - double z = bearing(2); - - double d1 = bearing.norm(); - double d2 = sqrt(x * x + y * y + (chi_ * d1 + z) * (chi_ * d1 + z)); - //double w1 = 0; - //if (alpha_ <= 0.5 && alpha_ >= 0) - //{ - //w1 = alpha_ / (1 - alpha_); - //} - //else if (alpha_ > 0.5) - //{ - //w1 = (1 - alpha_) / alpha_; - //} - //double w2 = (w1 + chi_) / sqrt(2 * w1 * chi_ + chi_ * chi_ + 1); - //if (z <= -w2 * d1) - //{ - //return false; - //} - if (alpha_ > 0.5) - { - if (z <= sinTheta_ * d1) - { - return false; - } - } - else - { - double w1 = alpha_ / (1 - alpha_); - double w2 = (w1 + chi_) / sqrt(2 * w1 * chi_ + chi_ * chi_ + 1); - if (z <= -w2 * d1) - { - return false; - } - } - - Vector3d bearing_h; - bearing_h << x, y, (alpha_ * d2 + (1 - alpha_) * (chi_ * d1 + z)); - Vector3d pixel_h = cameraMat_ * bearing_h; - pixel = pixel_h.hnormalized(); - if (pixel(0) > 2 * cx_ || pixel(0) < 0 || pixel(1) > 2 * cy_ || pixel(1) < 0) - { - return false; - } - return true; -} - -bool DoubleSphere::UnprojectToBearing(const Vector2d &pixel, Vector3d &bearing) const -{ - double mx = (pixel(0) - cx_) / fx_; - double my = (pixel(1) - cy_) / fy_; - - double r2 = mx * mx + my * my; - double beta1 = 1. - (2. * alpha_ - 1) * r2; - if (beta1 < 0) - { - return false; - } - double mz = (1. - alpha_ * alpha_ * r2) / (alpha_ * sqrt(beta1) + 1. - alpha_); - - double beta2 = mz * mz + (1. - chi_ * chi_) * r2; - - if (beta2 < 0) - { - return false; - } - - bearing << mx, my, mz; - - bearing *= (mz * chi_ + sqrt(mz * mz + (1. - chi_ * chi_) * r2)) / (mz * mz + r2); - bearing(2) -= chi_; - - return true; -} - -double DoubleSphere::GetFOV() -{ - double mx = cx_ / fx_; - double r2; - if (alpha_ > 0.5) - { - r2 = std::min(mx * mx, 1 / (2 * alpha_ - 1)); - } - else - { - r2 = mx * mx; - } - double mz = (1 - alpha_ * alpha_ * r2) / (alpha_ * sqrt(1 - (2 * alpha_ - 1) * r2) + 1 - alpha_); - double beta = (mz * chi_ + sqrt(mz * mz + (1 - chi_ * chi_) * r2)) / (mz * mz + r2); - return 2 * (M_PI / 2 - atan2(beta * mz - chi_, beta * mx)); -} - -} -} diff --git a/src/camera/double_sphere.h b/src/camera/double_sphere.h index 9cabbd1..c57f6ee 100644 --- a/src/camera/double_sphere.h +++ b/src/camera/double_sphere.h @@ -8,25 +8,132 @@ namespace omni_slam namespace camera { -class DoubleSphere : public CameraModel +template +class DoubleSphere : public CameraModel { public: - DoubleSphere(const double fx, const double fy, const double cx, const double cy, const double chi, const double alpha); + DoubleSphere(const T fx, const T fy, const T cx, const T cy, const T chi, const T alpha) + : CameraModel("DoubleSphere"), + fx_(fx), + fy_(fy), + cx_(cx), + cy_(cy), + chi_(chi), + alpha_(alpha) + { + cameraMat_ << fx_, 0., cx_, 0., fy_, cy_, 0., 0., 1.; + fov_ = GetFOV(); + T theta = M_PI / 2. - fov_ / 2.; + sinTheta_ = sin(theta); + } - bool ProjectToImage(const Vector3d &bearing, Vector2d &pixel) const; - bool UnprojectToBearing(const Vector2d &pixel, Vector3d &bearing) const; - double GetFOV(); + bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const + { + const T &x = bearing(0); + const T &y = bearing(1); + const T &z = bearing(2); + + T d1 = sqrt(x * x + y * y + z * z); + T d2 = sqrt(x * x + y * y + (chi_ * d1 + z) * (chi_ * d1 + z)); + //T w1 = 0; + //if (alpha_ <= 0.5 && alpha_ >= 0) + //{ + //w1 = alpha_ / (1 - alpha_); + //} + //else if (alpha_ > 0.5) + //{ + //w1 = (1 - alpha_) / alpha_; + //} + //T w2 = (w1 + chi_) / sqrt(2 * w1 * chi_ + chi_ * chi_ + 1); + //if (z <= -w2 * d1) + //{ + //return false; + //} + if (alpha_ > 0.5) + { + if (z <= sinTheta_ * d1) + { + return false; + } + } + else + { + T w1 = alpha_ / (1. - alpha_); + T w2 = (w1 + chi_) / sqrt(2. * w1 * chi_ + chi_ * chi_ + 1.); + if (z <= -w2 * d1) + { + return false; + } + } + + //Matrix bearing_h; + //bearing_h << x, y, (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z)); + //Matrix pixel_h = cameraMat_ * bearing_h; + //pixel = pixel_h.hnormalized(); + T denom = (alpha_ * d2 + (1. - alpha_) * (chi_ * d1 + z)); + pixel(0) = x * fx_ / denom + cx_; + pixel(1) = y * fy_ / denom + cy_; + if (pixel(0) > 2. * cx_ || pixel(0) < 0. || pixel(1) > 2. * cy_ || pixel(1) < 0.) + { + return false; + } + return true; + } + + bool UnprojectToBearing(const Matrix &pixel, Matrix &bearing) const + { + T mx = (pixel(0) - cx_) / fx_; + T my = (pixel(1) - cy_) / fy_; + + T r2 = mx * mx + my * my; + T beta1 = 1. - (2. * alpha_ - 1.) * r2; + if (beta1 < T(0)) + { + return false; + } + T mz = (1. - alpha_ * alpha_ * r2) / (alpha_ * sqrt(beta1) + 1. - alpha_); + + T beta2 = mz * mz + (1. - chi_ * chi_) * r2; + + if (beta2 < 0.) + { + return false; + } + + bearing << mx, my, mz; + + bearing *= (mz * chi_ + sqrt(mz * mz + (1. - chi_ * chi_) * r2)) / (mz * mz + r2); + bearing(2) -= chi_; + + return true; + } + T GetFOV() + { + T mx = cx_ / fx_; + T r2; + if (alpha_ > 0.5) + { + r2 = std::min(mx * mx, 1. / (2. * alpha_ - 1.)); + } + else + { + r2 = mx * mx; + } + T mz = (1. - alpha_ * alpha_ * r2) / (alpha_ * sqrt(1. - (2. * alpha_ - 1.) * r2) + 1. - alpha_); + T beta = (mz * chi_ + sqrt(mz * mz + (1. - chi_ * chi_) * r2)) / (mz * mz + r2); + return 2. * (M_PI / 2 - atan2(beta * mz - chi_, beta * mx)); + } private: - double fx_; - double fy_; - double cx_; - double cy_; - double chi_; - double alpha_; - double fov_; - double sinTheta_; - Matrix3d cameraMat_; + T fx_; + T fy_; + T cx_; + T cy_; + T chi_; + T alpha_; + T fov_; + T sinTheta_; + Matrix cameraMat_; }; } diff --git a/src/camera/perspective.cc b/src/camera/perspective.cc deleted file mode 100644 index 10755ed..0000000 --- a/src/camera/perspective.cc +++ /dev/null @@ -1,46 +0,0 @@ -#include "perspective.h" -#include - -namespace omni_slam -{ -namespace camera -{ - -Perspective::Perspective(const double fx, const double fy, const double cx, const double cy) - : CameraModel("Perspective"), - fx_(fx), - fy_(fy), - cx_(cx), - cy_(cy) -{ - cameraMat_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1; -} - -bool Perspective::ProjectToImage(const Vector3d &bearing, Vector2d &pixel) const -{ - Vector3d pixel_h = cameraMat_ * bearing; - pixel = pixel_h.hnormalized(); - if (pixel(0) > 2 * cx_ || pixel(0) < 0 || pixel(1) > 2 * cy_ || pixel(1) < 0) - { - return false; - } - return true; -} - -bool Perspective::UnprojectToBearing(const Vector2d &pixel, Vector3d &bearing) const -{ - Vector3d pixel_h; - double mx = (pixel(0) - cx_) / fx_; - double my = (pixel(1) - cy_) / fy_; - pixel_h << mx, my, 1; - bearing = 1. / sqrt(mx * mx + my * my + 1) * pixel_h; - return true; -} - -double Perspective::GetFOV() -{ - return 2 * atan(cx_ / fx_); -} - -} -} diff --git a/src/camera/perspective.h b/src/camera/perspective.h index 0ac75b6..0b3febe 100644 --- a/src/camera/perspective.h +++ b/src/camera/perspective.h @@ -8,21 +8,57 @@ namespace omni_slam namespace camera { -class Perspective : public CameraModel +template +class Perspective : public CameraModel { public: - Perspective(const double fx, const double fy, const double cx, const double cy); + Perspective(const T fx, const T fy, const T cx, const T cy) + : CameraModel("Perspective"), + fx_(fx), + fy_(fy), + cx_(cx), + cy_(cy) + { + cameraMat_ << fx_, 0., cx_, 0., fy_, cy_, 0., 0., 1.; + } - bool ProjectToImage(const Vector3d &bearing, Vector2d &pixel) const; - bool UnprojectToBearing(const Vector2d &pixel, Vector3d &bearing) const; - double GetFOV(); + bool ProjectToImage(const Matrix &bearing, Matrix &pixel) const + { + const T &x = bearing(0); + const T &y = bearing(1); + const T &z = bearing(2); + //Matrix pixel_h = cameraMat_ * bearing; + //pixel = pixel_h.hnormalized(); + pixel(0) = x * fx_ / z + cx_; + pixel(1) = y * fy_ / z + cy_; + if (pixel(0) > 2. * cx_ || pixel(0) < 0. || pixel(1) > 2. * cy_ || pixel(1) < 0.) + { + return false; + } + return true; + } + + bool UnprojectToBearing(const Matrix &pixel, Matrix &bearing) const + { + Matrix pixel_h; + T mx = (pixel(0) - cx_) / fx_; + T my = (pixel(1) - cy_) / fy_; + pixel_h << mx, my, 1; + bearing = 1. / sqrt(mx * mx + my * my + 1.) * pixel_h; + return true; + } + + T GetFOV() + { + return 2. * atan(cx_ / fx_); + } private: - double fx_; - double fy_; - double cx_; - double cy_; - Matrix3d cameraMat_; + T fx_; + T fy_; + T cx_; + T cy_; + Matrix cameraMat_; }; } diff --git a/src/data/frame.cc b/src/data/frame.cc index 03ec6d4..648d864 100644 --- a/src/data/frame.cc +++ b/src/data/frame.cc @@ -6,7 +6,7 @@ namespace omni_slam namespace data { -Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel &camera_model) +Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model) : id_(id), image_(image.clone()), depthImage_(depth_image.clone()), @@ -19,7 +19,7 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &camera_model) : id_(id), image_(image.clone()), depthImage_(depth_image.clone()), @@ -30,7 +30,7 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, ca hasDepth_ = true; } -Frame::Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel &camera_model) +Frame::Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model) : id_(id), image_(image.clone()), pose_(pose), @@ -42,7 +42,7 @@ Frame::Frame(const int id, cv::Mat &image, Matrix &pose, double t hasDepth_ = false; } -Frame::Frame(const int id, cv::Mat &image, double time, camera::CameraModel &camera_model) +Frame::Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model) : id_(id), timeSec_(time), cameraModel_(camera_model) @@ -100,7 +100,7 @@ const int Frame::GetID() const return id_; } -const camera::CameraModel& Frame::GetCameraModel() const +const camera::CameraModel<>& Frame::GetCameraModel() const { return cameraModel_; } @@ -136,13 +136,20 @@ void Frame::SetDepthImage(cv::Mat &depth_image) hasDepth_ = true; } -void Frame::SetEstimatedPose(Matrix &pose) +void Frame::SetEstimatedPose(const Matrix &pose) { poseEstimate_ = pose; invPoseEstimate_ = util::TFUtil::InversePoseMatrix(pose); hasPoseEstimate_ = true; } +void Frame::SetEstimatedInversePose(const Matrix &pose) +{ + invPoseEstimate_ = pose; + poseEstimate_ = util::TFUtil::InversePoseMatrix(pose); + hasPoseEstimate_ = true; +} + bool Frame::HasPose() const { return hasPose_; diff --git a/src/data/frame.h b/src/data/frame.h index 5ef11fc..40d1c93 100644 --- a/src/data/frame.h +++ b/src/data/frame.h @@ -15,17 +15,17 @@ namespace data class Frame { public: - Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel &camera_model); - Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel &camera_model); - Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel &camera_model); - Frame(const int id, cv::Mat &image, double time, camera::CameraModel &camera_model); + Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model); + Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model); + Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model); + Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model); Frame(const Frame &other); const Matrix& GetPose() const; const Matrix& GetInversePose() const; const cv::Mat& GetImage(); const cv::Mat& GetDepthImage(); - const camera::CameraModel& GetCameraModel() const; + const camera::CameraModel<>& GetCameraModel() const; const Matrix& GetEstimatedPose() const; const Matrix& GetEstimatedInversePose() const; const int GetID() const; @@ -36,7 +36,8 @@ class Frame void SetPose(Matrix &pose); void SetDepthImage(cv::Mat &depth_image); - void SetEstimatedPose(Matrix &pose); + void SetEstimatedPose(const Matrix &pose); + void SetEstimatedInversePose(const Matrix &pose); void CompressImages(); void DecompressImages(); @@ -53,7 +54,7 @@ class Frame Matrix poseEstimate_; Matrix invPoseEstimate_; double timeSec_; - camera::CameraModel &cameraModel_; + camera::CameraModel<> &cameraModel_; bool hasPose_; bool hasDepth_; diff --git a/src/data/landmark.cc b/src/data/landmark.cc index d40f261..2281273 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -72,7 +72,7 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const return &obs_[idToIndex_.at(frame_id)]; } -void Landmark::SetEstimatedPosition(Vector3d pos) +void Landmark::SetEstimatedPosition(const Vector3d &pos) { posEstimate_ = pos; hasPosEstimate_ = true; diff --git a/src/data/landmark.h b/src/data/landmark.h index aa63492..ee970d7 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -23,7 +23,7 @@ class Landmark const int GetFirstFrameID() const; const int GetNumObservations() const; const Feature* GetObservationByFrameID(const int frame_id) const; - void SetEstimatedPosition(Vector3d pos); + void SetEstimatedPosition(const Vector3d &pos); Vector3d GetGroundTruth() const; Vector3d GetEstimatedPosition() const; diff --git a/src/module/reconstruction_module.cc b/src/module/reconstruction_module.cc index bdcab93..bec2701 100644 --- a/src/module/reconstruction_module.cc +++ b/src/module/reconstruction_module.cc @@ -7,13 +7,14 @@ namespace omni_slam namespace module { -ReconstructionModule::ReconstructionModule(std::unique_ptr &triangulator) - : triangulator_(std::move(triangulator)) +ReconstructionModule::ReconstructionModule(std::unique_ptr &triangulator, std::unique_ptr &bundle_adjuster) + : triangulator_(std::move(triangulator)), + bundleAdjuster_(std::move(bundle_adjuster)) { } -ReconstructionModule::ReconstructionModule(std::unique_ptr &&triangulator) - : ReconstructionModule(triangulator) +ReconstructionModule::ReconstructionModule(std::unique_ptr &&triangulator, std::unique_ptr &&bundle_adjuster) + : ReconstructionModule(triangulator, bundle_adjuster) { } @@ -37,6 +38,19 @@ void ReconstructionModule::Update(std::vector &landmarks) lastLandmarksSize_ = landmarks.size(); } +void ReconstructionModule::BundleAdjust(std::vector &landmarks) +{ + bundleAdjuster_->Optimize(landmarks); + + for (int i = 0; i < landmarks.size(); i++) + { + if (landmarks[i].HasEstimatedPosition()) + { + visualization_.UpdatePoint(i, landmarks[i].GetEstimatedPosition()); + } + } +} + ReconstructionModule::Stats& ReconstructionModule::GetStats() { return stats_; diff --git a/src/module/reconstruction_module.h b/src/module/reconstruction_module.h index 0d62c7a..9c67095 100644 --- a/src/module/reconstruction_module.h +++ b/src/module/reconstruction_module.h @@ -6,6 +6,7 @@ #include #include "reconstruction/triangulator.h" +#include "optimization/bundle_adjuster.h" #include "data/landmark.h" #include @@ -25,10 +26,11 @@ class ReconstructionModule { }; - ReconstructionModule(std::unique_ptr &triangulator); - ReconstructionModule(std::unique_ptr &&triangulator); + ReconstructionModule(std::unique_ptr &triangulator, std::unique_ptr &bundle_adjuster); + ReconstructionModule(std::unique_ptr &&triangulator, std::unique_ptr &&bundle_adjuster); void Update(std::vector &landmarks); + void BundleAdjust(std::vector &landmarks); Stats& GetStats(); void Visualize(cv::Mat &img, pcl::PointCloud &cloud); @@ -50,6 +52,7 @@ class ReconstructionModule }; std::shared_ptr triangulator_; + std::shared_ptr bundleAdjuster_; Stats stats_; Visualization visualization_; diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc new file mode 100644 index 0000000..d58e408 --- /dev/null +++ b/src/optimization/bundle_adjuster.cc @@ -0,0 +1,141 @@ +#include "bundle_adjuster.h" + +#include "reprojection_error.h" + +namespace omni_slam +{ +namespace optimization +{ + +BundleAdjuster::BundleAdjuster(int max_iterations, bool log) +{ + problem_.reset(new ceres::Problem()); + solverOptions_.max_num_iterations = max_iterations; + solverOptions_.linear_solver_type = ceres::DENSE_SCHUR; + solverOptions_.minimizer_progress_to_stdout = log; + solverOptions_.logging_type = log ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT; + //solverOptions_.check_gradients = true; +} + +bool BundleAdjuster::Optimize(std::vector &landmarks) +{ + int numLandmarks = 0; + for (const data::Landmark &landmark : landmarks) + { + if (landmark.HasEstimatedPosition() || landmark.HasGroundTruth()) + { + numLandmarks++; + } + } + std::vector landmarkEstimates(3 * numLandmarks, 0); + std::map> framePoses; + std::map estFrames; + ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); + for (const data::Landmark &landmark : landmarks) + { + bool hasEstCameraPoses = false; + for (const data::Feature &feature : landmark.GetObservations()) + { + if (feature.GetFrame().HasEstimatedPose()) + { + hasEstCameraPoses = true; + } + } + if (!landmark.HasEstimatedPosition() && landmark.HasGroundTruth() && hasEstCameraPoses) + { + Vector3d gnd = landmark.GetGroundTruth(); + landmarkEstimates.push_back(gnd(0)); + landmarkEstimates.push_back(gnd(1)); + landmarkEstimates.push_back(gnd(2)); + problem_->AddParameterBlock(&landmarkEstimates[landmarkEstimates.size() - 3], 3); + problem_->SetParameterBlockConstant(&landmarkEstimates[landmarkEstimates.size() - 3]); + } + else if (landmark.HasEstimatedPosition()) + { + Vector3d est = landmark.GetEstimatedPosition(); + landmarkEstimates.push_back(est(0)); + landmarkEstimates.push_back(est(1)); + landmarkEstimates.push_back(est(2)); + problem_->AddParameterBlock(&landmarkEstimates[landmarkEstimates.size() - 3], 3); + } + else + { + continue; + } + for (const data::Feature &feature : landmark.GetObservations()) + { + if (!feature.GetFrame().HasEstimatedPose() && feature.GetFrame().HasPose()) + { + if (!landmark.HasEstimatedPosition()) + { + continue; + } + if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) + { + framePoses[feature.GetFrame().GetID()].insert(framePoses[feature.GetFrame().GetID()].end(), feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12); + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12); + problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()][0]); + } + } + else if (feature.GetFrame().HasEstimatedPose()) + { + if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) + { + framePoses[feature.GetFrame().GetID()].insert(framePoses[feature.GetFrame().GetID()].end(), feature.GetFrame().GetEstimatedInversePose().data(), feature.GetFrame().GetEstimatedInversePose().data() + 12); + problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12); + estFrames[feature.GetFrame().GetID()] = const_cast(&feature.GetFrame()); + } + } + else + { + continue; + } + ceres::CostFunction *cost_function = ReprojectionError::Create(feature); + problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]); + } + } + + ceres::Solver::Summary summary; + ceres::Solve(solverOptions_, problem_.get(), &summary); + if (solverOptions_.minimizer_progress_to_stdout) + { + std::cout << summary.FullReport() << std::endl; + } + if (!summary.IsSolutionUsable()) + { + return false; + } + + int inx = 0; + for (data::Landmark &landmark : landmarks) + { + bool hasEstCameraPoses = false; + for (const data::Feature &feature : landmark.GetObservations()) + { + if (feature.GetFrame().HasEstimatedPose()) + { + hasEstCameraPoses = true; + } + } + if (!landmark.HasEstimatedPosition() && landmark.HasGroundTruth() && hasEstCameraPoses) + { + inx++; + } + else if (landmark.HasEstimatedPosition()) + { + Vector3d est; + est << landmarkEstimates[inx * 3], landmarkEstimates[inx * 3 + 1], landmarkEstimates[inx * 3 + 2]; + landmark.SetEstimatedPosition(est); + inx++; + } + } + for (auto &frame : estFrames) + { + const Matrix pose = Map>(&framePoses[frame.first][0]); + frame.second->SetEstimatedInversePose(pose); + } + return true; +} + +} +} diff --git a/src/optimization/bundle_adjuster.h b/src/optimization/bundle_adjuster.h new file mode 100644 index 0000000..1ffe7a7 --- /dev/null +++ b/src/optimization/bundle_adjuster.h @@ -0,0 +1,26 @@ +#ifndef _BUNDLE_ADJUSTER_H_ +#define _BUNDLE_ADJUSTER_H_ + +#include +#include "data/landmark.h" + +namespace omni_slam +{ +namespace optimization +{ + +class BundleAdjuster +{ +public: + BundleAdjuster(int max_iterations = 500, bool log = false); + + bool Optimize(std::vector &landmarks); + + std::unique_ptr problem_; + ceres::Solver::Options solverOptions_; +}; + +} +} + +#endif /* _BUNDLE_ADJUSTER_H_ */ diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h new file mode 100644 index 0000000..33b80fa --- /dev/null +++ b/src/optimization/reprojection_error.h @@ -0,0 +1,55 @@ +#ifndef _REPROJECTION_ERROR_H_ +#define _REPROJECTION_ERROR_H_ + +#include "camera/camera_model.h" +#include "camera/double_sphere.h" +#include "data/feature.h" +#include +#include "util/tf_util.h" + +namespace omni_slam +{ +namespace optimization +{ + +class ReprojectionError +{ +public: + ReprojectionError(const data::Feature &feature) + : feature_(feature) + { + } + + template + bool operator()(const T* const camera_pose, const T* const point, T *reproj_error) const + { + Matrix reprojPoint; + //const camera::CameraModel &camera((camera::CameraModel&)feature_.GetFrame().GetCameraModel()); + const camera::DoubleSphere camera(T(295.936), T(295.936), T(512), T(512), T(0.3), T(0.6666667)); + const Matrix pose = Map>(camera_pose); + const Matrix worldPt = Map>(point); + const Matrix camPt = util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, worldPt)); + if (camera.ProjectToImage(camPt, reprojPoint)) + { + reproj_error[0] = reprojPoint(0) - T(feature_.GetKeypoint().pt.x); + reproj_error[1] = reprojPoint(1) - T(feature_.GetKeypoint().pt.y); + return true; + } + reproj_error[0] = T(0.); + reproj_error[1] = T(0.); + return true; + } + + static ceres::CostFunction* Create(const data::Feature &feature) + { + return new ceres::AutoDiffCostFunction(new ReprojectionError(feature)); + } + +private: + const data::Feature feature_; +}; + +} +} + +#endif /* _REPROJECTION_ERROR_H_ */ diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc index b7d3c8a..4c7426e 100644 --- a/src/ros/eval_base.cc +++ b/src/ros/eval_base.cc @@ -27,11 +27,11 @@ EvalBase::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_priv if (cameraModel == "double_sphere") { - cameraModel_.reset(new camera::DoubleSphere(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"], cameraParams_["chi"], cameraParams_["alpha"])); + cameraModel_.reset(new camera::DoubleSphere<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"], cameraParams_["chi"], cameraParams_["alpha"])); } else if (cameraModel == "perspective") { - cameraModel_.reset(new camera::Perspective(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"])); + cameraModel_.reset(new camera::Perspective<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"])); } else { @@ -82,6 +82,10 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, const sens frameNum_++; } +void EvalBase::Finish() +{ +} + bool EvalBase::GetAttributes(std::map &attributes) { return false; @@ -130,10 +134,12 @@ void EvalBase::Run() geometry_msgs::PoseStamped::ConstPtr poseMsg = nullptr; int runNext = 0; int skip = 0; + int finished = true; for (rosbag::MessageInstance const m : rosbag::View(bag)) { if (!::ros::ok()) { + finished = false; break; } if (m.getTopic() == depthImageTopic_) @@ -164,6 +170,10 @@ void EvalBase::Run() } } } + if (finished) + { + Finish(); + } ROS_INFO("Saving results..."); std::map>> results; diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h index a37c3ea..d924792 100644 --- a/src/ros/eval_base.h +++ b/src/ros/eval_base.h @@ -38,7 +38,7 @@ class EvalBase image_transport::ImageTransport imageTransport_; - std::unique_ptr cameraModel_; + std::unique_ptr> cameraModel_; int frameNum_{0}; @@ -47,6 +47,7 @@ class EvalBase virtual void ProcessFrame(std::unique_ptr &&frame) = 0; virtual void GetResultsData(std::map>> &data) = 0; + virtual void Finish(); virtual bool GetAttributes(std::map &attributes); virtual bool GetAttributes(std::map &attributes); virtual void Visualize(cv_bridge::CvImagePtr &base_img); diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc index db4de0c..ce9e102 100644 --- a/src/ros/reconstruction_eval.cc +++ b/src/ros/reconstruction_eval.cc @@ -1,6 +1,7 @@ #include "reconstruction_eval.h" #include "reconstruction/triangulator.h" +#include "optimization/bundle_adjuster.h" #include "module/tracking_module.h" #include @@ -18,13 +19,18 @@ ReconstructionEval::ReconstructionEval(const ::ros::NodeHandle &nh, const ::ros: { double maxReprojError; double minTriAngle; + int baMaxIter; + bool logCeres; nhp_.param("max_reprojection_error", maxReprojError, 0.0); nhp_.param("min_triangulation_angle", minTriAngle, 1.0); + nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500); + nhp_.param("bundle_adjustment_logging", logCeres, false); unique_ptr triangulator(new reconstruction::Triangulator(maxReprojError, minTriAngle)); + unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres)); - reconstructionModule_.reset(new module::ReconstructionModule(triangulator)); + reconstructionModule_.reset(new module::ReconstructionModule(triangulator, bundleAdjuster)); } void ReconstructionEval::InitPublishers() @@ -41,6 +47,20 @@ void ReconstructionEval::ProcessFrame(unique_ptr &&frame) reconstructionModule_->Update(trackingModule_->GetLandmarks()); } +void ReconstructionEval::Finish() +{ + reconstructionModule_->BundleAdjust(trackingModule_->GetLandmarks()); + + pcl::PointCloud cloud; + cv::Mat noarr; + reconstructionModule_->Visualize(noarr, cloud); + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + msg.header.frame_id = "map"; + msg.header.stamp = ::ros::Time::now(); + pointCloudPublisher_.publish(msg); +} + void ReconstructionEval::GetResultsData(std::map>> &data) { module::ReconstructionModule::Stats &stats = reconstructionModule_->GetStats(); diff --git a/src/ros/reconstruction_eval.h b/src/ros/reconstruction_eval.h index 8e919f4..37cbc93 100644 --- a/src/ros/reconstruction_eval.h +++ b/src/ros/reconstruction_eval.h @@ -22,6 +22,7 @@ class ReconstructionEval : public TrackingEval void InitPublishers(); void ProcessFrame(std::unique_ptr &&frame); + void Finish(); void GetResultsData(std::map>> &data); void Visualize(cv_bridge::CvImagePtr &base_img); diff --git a/src/util/math_util.cc b/src/util/math_util.cc deleted file mode 100644 index 0c7c6f9..0000000 --- a/src/util/math_util.cc +++ /dev/null @@ -1,37 +0,0 @@ -#include "math_util.h" -#include - -namespace omni_slam -{ -namespace util -{ - -double MathUtil::FastAtan2(double y, double x) -{ - const float ONEQTR_PI = M_PI / 4.0; - const float THRQTR_PI = 3.0 * M_PI / 4.0; - float r, angle; - float abs_y = fabs(y) + 1e-10f; - if (x < 0.0f) - { - r = (x + abs_y) / (abs_y - x); - angle = THRQTR_PI; - } - else - { - r = (x - abs_y) / (x + abs_y); - angle = ONEQTR_PI; - } - angle += (0.1963f * r * r - 0.9817f) * r; - if (y < 0.0f) - { - return -angle; - } - else - { - return angle; - } -} - -} -} diff --git a/src/util/math_util.h b/src/util/math_util.h index 4057837..a790be1 100644 --- a/src/util/math_util.h +++ b/src/util/math_util.h @@ -9,7 +9,33 @@ namespace util class MathUtil { public: - static double FastAtan2(double y, double x); + template + static T FastAtan2(T y, T x) + { + const T ONEQTR_PI = M_PI / 4.0; + const T THRQTR_PI = 3.0 * M_PI / 4.0; + T r, angle; + T abs_y = fabs(y) + 1e-10f; + if (x < 0.0f) + { + r = (x + abs_y) / (abs_y - x); + angle = THRQTR_PI; + } + else + { + r = (x - abs_y) / (x + abs_y); + angle = ONEQTR_PI; + } + angle += (0.1963f * r * r - 0.9817f) * r; + if (y < 0.0f) + { + return -angle; + } + else + { + return angle; + } + } }; } diff --git a/src/util/tf_util.cc b/src/util/tf_util.cc deleted file mode 100644 index d03ec9a..0000000 --- a/src/util/tf_util.cc +++ /dev/null @@ -1,57 +0,0 @@ -#include "tf_util.h" - -namespace omni_slam -{ -namespace util -{ - -Vector3d TFUtil::CameraFrameToWorldFrame(Vector3d cameraFramePt) -{ - Vector3d worldFramePt; - worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1); - return worldFramePt; -} - -Vector3d TFUtil::WorldFrameToCameraFrame(Vector3d worldFramePt) -{ - Vector3d cameraFramePt; - cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1); - return cameraFramePt; -} - -Matrix TFUtil::InversePoseMatrix(Matrix poseMat) -{ - Matrix3d rot = poseMat.block<3, 3>(0, 0); - Vector3d t = poseMat.block<3, 1>(0, 3); - Matrix invMat; - invMat.block<3, 3>(0, 0) = rot.transpose(); - invMat.block<3, 1>(0, 3) = -rot.transpose() * t; - return invMat; -} - -Matrix3d TFUtil::GetRotationFromPoseMatrix(Matrix poseMat) -{ - return poseMat.block<3, 3>(0, 0); -} - -Vector3d TFUtil::TransformPoint(Matrix tf, Vector3d pt) -{ - Vector4d pt_h = pt.homogeneous(); - return tf * pt_h; -} - -Vector3d TFUtil::RotatePoint(Matrix3d rot, Vector3d pt) -{ - return rot * pt; -} - -Matrix TFUtil::QuaternionTranslationToPoseMatrix(Quaterniond quat, Vector3d translation) -{ - Matrix pose; - pose.block<3, 3>(0, 0) = quat.normalized().toRotationMatrix(); - pose.block<3, 1>(0, 3) = translation; - return pose; -} - -} -} diff --git a/src/util/tf_util.h b/src/util/tf_util.h index 4d3697f..42419d8 100644 --- a/src/util/tf_util.h +++ b/src/util/tf_util.h @@ -13,13 +13,54 @@ namespace util class TFUtil { public: - static Vector3d CameraFrameToWorldFrame(Vector3d cameraFramePt); - static Vector3d WorldFrameToCameraFrame(Vector3d worldFramePt); - static Matrix InversePoseMatrix(Matrix poseMat); - static Matrix3d GetRotationFromPoseMatrix(Matrix poseMat); - static Vector3d TransformPoint(Matrix tf, Vector3d pt); - static Vector3d RotatePoint(Matrix3d rot, Vector3d pt); - static Matrix QuaternionTranslationToPoseMatrix(Quaterniond quat, Vector3d translation); + template + static Matrix CameraFrameToWorldFrame(Matrix cameraFramePt) + { + Matrix worldFramePt; + worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1); + return worldFramePt; + } + template + static Matrix WorldFrameToCameraFrame(Matrix worldFramePt) + { + Matrix cameraFramePt; + cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1); + return cameraFramePt; + } + template + static Matrix InversePoseMatrix(Matrix poseMat) + { + Matrix rot = poseMat.template block<3, 3>(0, 0); + Matrix t = poseMat.template block<3, 1>(0, 3); + Matrix invMat; + invMat.template block<3, 3>(0, 0) = rot.transpose(); + invMat.template block<3, 1>(0, 3) = -rot.transpose() * t; + return invMat; + } + template + static Matrix GetRotationFromPoseMatrix(Matrix poseMat) + { + return poseMat.template block<3, 3>(0, 0); + } + template + static Matrix TransformPoint(Matrix tf, Matrix pt) + { + Matrix pt_h = pt.homogeneous(); + return tf * pt_h; + } + template + static Matrix RotatePoint(Matrix rot, Matrix pt) + { + return rot * pt; + } + template + static Matrix QuaternionTranslationToPoseMatrix(Quaternion quat, Matrix translation) + { + Matrix pose; + pose.template block<3, 3>(0, 0) = quat.normalized().toRotationMatrix(); + pose.template block<3, 1>(0, 3) = translation; + return pose; + } }; }