From 0b9db2ba07be4f3d231d8896726b4cdce733230d Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Sun, 15 Sep 2019 22:41:11 -0400 Subject: [PATCH] odom bundle adjustment --- launch/odometry_eval.launch | 10 +++-- launch/reconstruction_eval.launch | 4 +- src/data/frame.cc | 44 ++++++++++++++---- src/data/frame.h | 17 +++++-- src/data/landmark.cc | 33 +++++++++----- src/data/landmark.h | 10 ++++- src/odometry/pnp.cc | 26 ++++++++--- src/odometry/pnp.h | 3 +- src/optimization/bundle_adjuster.cc | 22 +++++---- src/optimization/bundle_adjuster.h | 5 ++- src/reconstruction/triangulator.cc | 8 +++- src/ros/eval_base.cc | 4 +- src/ros/eval_base.h | 2 - src/ros/odometry_eval.cc | 69 +++++++++++++++++++++++++++-- src/ros/odometry_eval.h | 3 ++ src/ros/reconstruction_eval.cc | 7 ++- 16 files changed, 211 insertions(+), 56 deletions(-) diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 95d567e..ee07bc6 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -11,8 +11,10 @@ - + + + camera_model: '$(arg camera_model)' @@ -24,10 +26,12 @@ tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 min_features_per_region: 10 - pnp_inlier_threshold: 5.0 - pnp_iterations: 1000 + pnp_inlier_threshold: 3.0 + pnp_iterations: 2000 bundle_adjustment_max_iterations: 1000 + bundle_adjustment_loss_coefficient: 0.05 bundle_adjustment_logging: true + bundle_adjustment_num_threads: 20 diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index 1ede7fb..ef58dca 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -23,10 +23,12 @@ tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 min_features_per_region: 10 - max_reprojection_error: 0.0 + max_reprojection_error: 5.0 min_triangulation_angle: 3.0 bundle_adjustment_max_iterations: 1000 + bundle_adjustment_loss_coefficient: 0.1 bundle_adjustment_logging: true + bundle_adjustment_num_threads: 20 diff --git a/src/data/frame.cc b/src/data/frame.cc index 648d864..663ef4b 100644 --- a/src/data/frame.cc +++ b/src/data/frame.cc @@ -6,8 +6,10 @@ 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) - : id_(id), +int Frame::lastFrameId_ = 0; + +Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model) + : id_(lastFrameId_++), image_(image.clone()), depthImage_(depth_image.clone()), pose_(pose), @@ -19,8 +21,8 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &camera_model) - : id_(id), +Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model) + : id_(lastFrameId_++), image_(image.clone()), depthImage_(depth_image.clone()), timeSec_(time), @@ -30,8 +32,8 @@ 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) - : id_(id), +Frame::Frame(cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model) + : id_(lastFrameId_++), image_(image.clone()), pose_(pose), invPose_(util::TFUtil::InversePoseMatrix(pose)), @@ -42,8 +44,8 @@ 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) - : id_(id), +Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model) + : id_(lastFrameId_++), timeSec_(time), cameraModel_(camera_model) { @@ -136,6 +138,14 @@ void Frame::SetDepthImage(cv::Mat &depth_image) hasDepth_ = true; } +void Frame::SetEstimatedPose(const Matrix &pose, const std::vector &landmark_ids) +{ + poseEstimate_ = pose; + invPoseEstimate_ = util::TFUtil::InversePoseMatrix(pose); + estLandmarkIds_ = std::unordered_set(landmark_ids.begin(), landmark_ids.end()); + hasPoseEstimate_ = true; +} + void Frame::SetEstimatedPose(const Matrix &pose) { poseEstimate_ = pose; @@ -143,6 +153,14 @@ void Frame::SetEstimatedPose(const Matrix &pose) hasPoseEstimate_ = true; } +void Frame::SetEstimatedInversePose(const Matrix &pose, const std::vector &landmark_ids) +{ + invPoseEstimate_ = pose; + poseEstimate_ = util::TFUtil::InversePoseMatrix(pose); + estLandmarkIds_ = std::unordered_set(landmark_ids.begin(), landmark_ids.end()); + hasPoseEstimate_ = true; +} + void Frame::SetEstimatedInversePose(const Matrix &pose) { invPoseEstimate_ = pose; @@ -165,6 +183,11 @@ bool Frame::HasEstimatedPose() const return hasPoseEstimate_; } +bool Frame::IsEstimatedByLandmark(const int landmark_id) const +{ + return estLandmarkIds_.find(landmark_id) != estLandmarkIds_.end(); +} + void Frame::CompressImages() { if (isCompressed_) @@ -203,5 +226,10 @@ bool Frame::IsCompressed() const return isCompressed_; } +const double Frame::GetTime() const +{ + return timeSec_; +} + } } diff --git a/src/data/frame.h b/src/data/frame.h index 40d1c93..7ec88eb 100644 --- a/src/data/frame.h +++ b/src/data/frame.h @@ -3,6 +3,7 @@ #include #include +#include #include "camera/camera_model.h" using namespace Eigen; @@ -15,10 +16,10 @@ 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(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model); Frame(const Frame &other); const Matrix& GetPose() const; @@ -29,14 +30,18 @@ class Frame const Matrix& GetEstimatedPose() const; const Matrix& GetEstimatedInversePose() const; const int GetID() const; + const double GetTime() const; bool HasPose() const; bool HasDepthImage() const; bool HasEstimatedPose() const; + bool IsEstimatedByLandmark(const int landmark_id) const; void SetPose(Matrix &pose); void SetDepthImage(cv::Mat &depth_image); + void SetEstimatedPose(const Matrix &pose, const std::vector &landmark_ids); void SetEstimatedPose(const Matrix &pose); + void SetEstimatedInversePose(const Matrix &pose, const std::vector &landmark_ids); void SetEstimatedInversePose(const Matrix &pose); void CompressImages(); @@ -56,11 +61,15 @@ class Frame double timeSec_; camera::CameraModel<> &cameraModel_; + std::unordered_set estLandmarkIds_; + bool hasPose_; bool hasDepth_; bool hasPoseEstimate_{false}; bool isCompressed_{false}; + + static int lastFrameId_; }; } diff --git a/src/data/landmark.cc b/src/data/landmark.cc index 7eb0a5c..a2dbff2 100644 --- a/src/data/landmark.cc +++ b/src/data/landmark.cc @@ -5,7 +5,10 @@ namespace omni_slam namespace data { +int Landmark::lastLandmarkId_ = 0; + Landmark::Landmark() + : id_(lastLandmarkId_++) { } @@ -21,7 +24,7 @@ void Landmark::AddObservation(Feature obs, bool compute_gnd) if (obs.HasEstimatedWorldPoint()) { posEstimate_ = obs.GetEstimatedWorldPoint(); - obsForEst_.push_back(obs); + estFrameIds_.insert(obs.GetFrame().GetID()); hasPosEstimate_ = true; } } @@ -38,11 +41,6 @@ const std::vector& Landmark::GetObservations() const return obs_; } -const std::vector& Landmark::GetObservationsForEstimate() const -{ - return obsForEst_; -} - std::vector& Landmark::GetObservations() { return obs_; @@ -83,18 +81,24 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const return &obs_[idToIndex_.at(frame_id)]; } +void Landmark::SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids) +{ + posEstimate_ = pos; + estFrameIds_ = std::unordered_set(frame_ids.begin(), frame_ids.end()); + hasPosEstimate_ = true; +} + void Landmark::SetEstimatedPosition(const Vector3d &pos) { posEstimate_ = pos; - obsForEst_.clear(); - obsForEst_.reserve(obs_.size()); - for (data::Feature &feat : obs_) - { - obsForEst_.push_back(feat); - } hasPosEstimate_ = true; } +const int Landmark::GetID() const +{ + return id_; +} + Vector3d Landmark::GetGroundTruth() const { return groundTruth_; @@ -115,5 +119,10 @@ bool Landmark::HasEstimatedPosition() const return hasPosEstimate_; } +bool Landmark::IsEstimatedByFrame(const int frame_id) const +{ + return estFrameIds_.find(frame_id) != estFrameIds_.end(); +} + } } diff --git a/src/data/landmark.h b/src/data/landmark.h index f68c71e..db5b9e4 100644 --- a/src/data/landmark.h +++ b/src/data/landmark.h @@ -3,6 +3,7 @@ #include "feature.h" #include +#include #include using namespace Eigen; @@ -19,26 +20,31 @@ class Landmark void AddObservation(Feature obs, bool compute_gnd = true); const std::vector& GetObservations() const; std::vector& GetObservations(); - const std::vector& GetObservationsForEstimate() const; bool IsObservedInFrame(const int frame_id) const; const int GetFirstFrameID() const; const int GetNumObservations() const; const Feature* GetObservationByFrameID(const int frame_id) const; + void SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids); void SetEstimatedPosition(const Vector3d &pos); + const int GetID() const; Vector3d GetGroundTruth() const; Vector3d GetEstimatedPosition() const; bool HasGroundTruth() const; bool HasEstimatedPosition() const; + bool IsEstimatedByFrame(const int frame_id) const; private: + const int id_; std::vector obs_; - std::vector obsForEst_; + std::unordered_set estFrameIds_; std::map idToIndex_; Vector3d groundTruth_; Vector3d posEstimate_; bool hasGroundTruth_{false}; bool hasPosEstimate_{false}; + + static int lastLandmarkId_; }; } diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index fe0e5cf..7de3d35 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -16,9 +16,10 @@ namespace omni_slam namespace odometry { -PNP::PNP(int ransac_iterations, double reprojection_threshold) +PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads) : ransacIterations_(ransac_iterations), - reprojThreshold_(reprojection_threshold) + reprojThreshold_(reprojection_threshold), + numRefineThreads_(num_refine_threads) { } @@ -28,6 +29,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram std::vector yns; std::vector ys; std::vector features; + std::map indexToId; for (const data::Landmark &landmark : landmarks) { if (landmark.IsObservedInFrame(frame.GetID())) @@ -50,6 +52,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram yns.push_back(pix); ys.push_back(feat->GetBearing()); features.push_back(feat); + indexToId[xs.size() - 1] = landmark.GetID(); } } Matrix pose; @@ -59,13 +62,20 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram { Refine(xs, features, indices, pose); } - frame.SetEstimatedInversePose(pose); + std::vector inlierIds; + inlierIds.reserve(indices.size()); + for (int inx : indices) + { + inlierIds.push_back(indexToId[inx]); + } + frame.SetEstimatedInversePose(pose, inlierIds); return inliers; } int PNP::RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const { int maxInliers = 0; + #pragma omp parallel for for (int i = 0; i < ransacIterations_; i++) { std::random_device rd; @@ -87,10 +97,13 @@ int PNP::RANSAC(const std::vector &xs, const std::vector &ys } int inliers = GetInlierIndices(xs, yns, iterPose, camera_model).size(); - if (inliers > maxInliers) + #pragma omp critical { - maxInliers = inliers; - pose = iterPose; + if (inliers > maxInliers) + { + maxInliers = inliers; + pose = iterPose; + } } } return maxInliers; @@ -133,6 +146,7 @@ bool PNP::Refine(const std::vector &xs, const std::vector &landmarks, data::Frame &frame) const; private: @@ -28,6 +28,7 @@ class PNP int ransacIterations_; double reprojThreshold_; + int numRefineThreads_; }; } diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc index 39c2506..5fc702b 100644 --- a/src/optimization/bundle_adjuster.cc +++ b/src/optimization/bundle_adjuster.cc @@ -10,12 +10,15 @@ namespace omni_slam namespace optimization { -BundleAdjuster::BundleAdjuster(int max_iterations, bool log) +BundleAdjuster::BundleAdjuster(int max_iterations, double loss_coeff, int num_threads, bool log) + : lossCoeff_(loss_coeff) { problem_.reset(new ceres::Problem()); solverOptions_.max_num_iterations = max_iterations; solverOptions_.linear_solver_type = ceres::ITERATIVE_SCHUR; solverOptions_.preconditioner_type = ceres::SCHUR_JACOBI; + solverOptions_.num_threads = num_threads; + solverOptions_.use_inner_iterations = true; solverOptions_.minimizer_progress_to_stdout = log; solverOptions_.logging_type = log ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT; } @@ -26,7 +29,7 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) landmarkEstimates.reserve(3 * landmarks.size()); std::map, std::vector>> framePoses; std::map estFrames; - ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0); + ceres::LossFunction *loss_function = new ceres::HuberLoss(lossCoeff_); for (const data::Landmark &landmark : landmarks) { bool hasEstCameraPoses = false; @@ -58,8 +61,7 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) { continue; } - const std::vector obs = landmark.HasEstimatedPosition() ? landmark.GetObservationsForEstimate() : landmark.GetObservations(); - for (const data::Feature &feature : obs) + for (const data::Feature &feature : landmark.GetObservations()) { if (!feature.GetFrame().HasEstimatedPose() && feature.GetFrame().HasPose()) { @@ -67,6 +69,10 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) { continue; } + if (!landmark.IsEstimatedByFrame(feature.GetFrame().GetID())) + { + continue; + } if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) { const Matrix &pose = feature.GetFrame().GetInversePose(); @@ -82,6 +88,10 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } else if (feature.GetFrame().HasEstimatedPose()) { + if (!(feature.GetFrame().IsEstimatedByLandmark(landmark.GetID()) || (landmark.HasEstimatedPosition() && landmark.IsEstimatedByFrame(feature.GetFrame().GetID())))) + { + continue; + } if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end()) { const Matrix &pose = feature.GetFrame().GetInversePose(); @@ -115,10 +125,6 @@ bool BundleAdjuster::Optimize(std::vector &landmarks) } } - 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) diff --git a/src/optimization/bundle_adjuster.h b/src/optimization/bundle_adjuster.h index 1ffe7a7..3713a82 100644 --- a/src/optimization/bundle_adjuster.h +++ b/src/optimization/bundle_adjuster.h @@ -12,12 +12,15 @@ namespace optimization class BundleAdjuster { public: - BundleAdjuster(int max_iterations = 500, bool log = false); + BundleAdjuster(int max_iterations = 500, double loss_coeff = 0.1, int num_threads = 1, bool log = false); bool Optimize(std::vector &landmarks); +private: std::unique_ptr problem_; ceres::Solver::Options solverOptions_; + + double lossCoeff_; }; } diff --git a/src/reconstruction/triangulator.cc b/src/reconstruction/triangulator.cc index 4172a7c..4ba1256 100644 --- a/src/reconstruction/triangulator.cc +++ b/src/reconstruction/triangulator.cc @@ -25,7 +25,13 @@ int Triangulator::Triangulate(std::vector &landmarks) const { if (CheckReprojectionErrors(landmark.GetObservations(), point)) { - landmark.SetEstimatedPosition(point); + std::vector frameIds; + frameIds.reserve(landmark.GetObservations().size()); + for (data::Feature &obs : landmark.GetObservations()) + { + frameIds.push_back(obs.GetFrame().GetID()); + } + landmark.SetEstimatedPosition(point, frameIds); numSuccess++; } } diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc index 4c7426e..5b3e67c 100644 --- a/src/ros/eval_base.cc +++ b/src/ros/eval_base.cc @@ -75,11 +75,9 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, const sens cv::Mat depthFloatImg; cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535); - ProcessFrame(std::unique_ptr(new data::Frame(frameNum_, monoImg, depthFloatImg, posemat, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, depthFloatImg, posemat, pose->header.stamp.toSec(), *cameraModel_))); Visualize(cvImage); - - frameNum_++; } void EvalBase::Finish() diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h index d924792..4b5c5dc 100644 --- a/src/ros/eval_base.h +++ b/src/ros/eval_base.h @@ -40,8 +40,6 @@ class EvalBase std::unique_ptr> cameraModel_; - int frameNum_{0}; - private: void FrameCallback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth_image, const geometry_msgs::PoseStamped::ConstPtr &pose); diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index 0dc5b56..b6f492e 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -5,6 +5,7 @@ #include "module/tracking_module.h" #include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/Path.h" using namespace std; @@ -19,15 +20,19 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle double reprojThresh; int iterations; int baMaxIter; + double baLossCoeff; bool logCeres; + int numCeresThreads; nhp_.param("pnp_inlier_threshold", reprojThresh, 10.); nhp_.param("pnp_iterations", iterations, 1000); nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500); + nhp_.param("bundle_adjustment_loss_coefficient", baLossCoeff, 0.1); nhp_.param("bundle_adjustment_logging", logCeres, false); + nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1); - unique_ptr pnp(new odometry::PNP(iterations, reprojThresh)); - unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres)); + unique_ptr pnp(new odometry::PNP(iterations, reprojThresh, numCeresThreads)); + unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres)); odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster)); } @@ -37,10 +42,16 @@ void OdometryEval::InitPublishers() TrackingEval::InitPublishers(); string outputTopic; string outputGndTopic; + string outputPathTopic; + string outputPathGndTopic; nhp_.param("odometry_estimate_topic", outputTopic, string("/omni_slam/odometry")); nhp_.param("odometry_ground_truth_topic", outputGndTopic, string("/omni_slam/odometry_truth")); + nhp_.param("path_estimate_topic", outputPathTopic, string("/omni_slam/odometry_path")); + nhp_.param("path_ground_truth_topic", outputPathGndTopic, string("/omni_slam/odometry_path_truth")); odometryPublisher_ = nh_.advertise(outputTopic, 2); odometryGndPublisher_ = nh_.advertise(outputGndTopic, 2); + pathPublisher_ = nh_.advertise(outputPathTopic, 2); + pathGndPublisher_ = nh_.advertise(outputPathGndTopic, 2); } void OdometryEval::ProcessFrame(unique_ptr &&frame) @@ -52,7 +63,9 @@ void OdometryEval::ProcessFrame(unique_ptr &&frame) void OdometryEval::Finish() { + ROS_INFO("Performing bundle adjustment..."); odometryModule_->BundleAdjust(trackingModule_->GetLandmarks()); + PublishOdometry(); } void OdometryEval::GetResultsData(std::map>> &data) @@ -63,8 +76,12 @@ void OdometryEval::GetResultsData(std::mapGetFrames().back()->HasEstimatedPose()) { @@ -77,6 +94,7 @@ void OdometryEval::Visualize(cv_bridge::CvImagePtr &base_img) poseMsg.pose.orientation.y = quat.normalized().y(); poseMsg.pose.orientation.z = quat.normalized().z(); poseMsg.pose.orientation.w = quat.normalized().w(); + poseMsg.header.stamp = ::ros::Time(trackingModule_->GetFrames().back()->GetTime()); odometryPublisher_.publish(poseMsg); } const Matrix &poseGnd = trackingModule_->GetFrames().back()->GetPose(); @@ -88,7 +106,52 @@ void OdometryEval::Visualize(cv_bridge::CvImagePtr &base_img) poseMsg.pose.orientation.y = quatGnd.normalized().y(); poseMsg.pose.orientation.z = quatGnd.normalized().z(); poseMsg.pose.orientation.w = quatGnd.normalized().w(); + poseMsg.header.stamp = ::ros::Time(trackingModule_->GetFrames().back()->GetTime()); odometryGndPublisher_.publish(poseMsg); + + nav_msgs::Path path; + path.header.stamp = ::ros::Time::now(); + path.header.frame_id = "map"; + nav_msgs::Path pathGnd; + pathGnd.header.stamp = ::ros::Time::now(); + pathGnd.header.frame_id = "map"; + for (const std::unique_ptr &frame : trackingModule_->GetFrames()) + { + if (frame->HasEstimatedPose()) + { + geometry_msgs::PoseStamped poseMsg; + poseMsg.header.stamp = ::ros::Time(frame->GetTime()); + poseMsg.header.frame_id = "map"; + const Matrix &pose = frame->GetEstimatedPose(); + poseMsg.pose.position.x = pose(0, 3); + poseMsg.pose.position.y = pose(1, 3); + poseMsg.pose.position.z = pose(2, 3); + Quaterniond quat(pose.block<3, 3>(0, 0)); + poseMsg.pose.orientation.x = quat.normalized().x(); + poseMsg.pose.orientation.y = quat.normalized().y(); + poseMsg.pose.orientation.z = quat.normalized().z(); + poseMsg.pose.orientation.w = quat.normalized().w(); + path.poses.push_back(poseMsg); + } + if (frame->HasPose()) + { + geometry_msgs::PoseStamped poseMsg; + poseMsg.header.stamp = ::ros::Time(frame->GetTime()); + poseMsg.header.frame_id = "map"; + const Matrix &pose = frame->GetPose(); + poseMsg.pose.position.x = pose(0, 3); + poseMsg.pose.position.y = pose(1, 3); + poseMsg.pose.position.z = pose(2, 3); + Quaterniond quat(pose.block<3, 3>(0, 0)); + poseMsg.pose.orientation.x = quat.normalized().x(); + poseMsg.pose.orientation.y = quat.normalized().y(); + poseMsg.pose.orientation.z = quat.normalized().z(); + poseMsg.pose.orientation.w = quat.normalized().w(); + pathGnd.poses.push_back(poseMsg); + } + } + pathPublisher_.publish(path); + pathGndPublisher_.publish(pathGnd); } } diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h index 2de3e2d..6ca98fc 100644 --- a/src/ros/odometry_eval.h +++ b/src/ros/odometry_eval.h @@ -25,11 +25,14 @@ class OdometryEval : public TrackingEval void Finish(); void GetResultsData(std::map>> &data); void Visualize(cv_bridge::CvImagePtr &base_img); + void PublishOdometry(); std::unique_ptr odometryModule_; ::ros::Publisher odometryPublisher_; ::ros::Publisher odometryGndPublisher_; + ::ros::Publisher pathPublisher_; + ::ros::Publisher pathGndPublisher_; }; } diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc index 0e2f9be..2260da7 100644 --- a/src/ros/reconstruction_eval.cc +++ b/src/ros/reconstruction_eval.cc @@ -20,15 +20,19 @@ ReconstructionEval::ReconstructionEval(const ::ros::NodeHandle &nh, const ::ros: double maxReprojError; double minTriAngle; int baMaxIter; + double baLossCoeff; bool logCeres; + int numCeresThreads; 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_loss_coefficient", baLossCoeff, 0.1); nhp_.param("bundle_adjustment_logging", logCeres, false); + nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1); unique_ptr triangulator(new reconstruction::Triangulator(maxReprojError, minTriAngle)); - unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres)); + unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres)); reconstructionModule_.reset(new module::ReconstructionModule(triangulator, bundleAdjuster)); } @@ -50,6 +54,7 @@ void ReconstructionEval::ProcessFrame(unique_ptr &&frame) void ReconstructionEval::Finish() { + ROS_INFO("Performing bundle adjustment..."); reconstructionModule_->BundleAdjust(trackingModule_->GetLandmarks()); pcl::PointCloud cloud;