diff --git a/CMakeLists.txt b/CMakeLists.txt index d02ace0..b7c53a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,6 @@ project(omni_slam_eval) set(CMAKE_VERBOSE_MAKEFILE FALSE) -add_definitions("-std=c++17") if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) endif (NOT CMAKE_BUILD_TYPE) @@ -109,6 +108,16 @@ add_executable(omni_slam_stereo_eval_node src/omni_slam_stereo_eval_node.cc ) +add_executable(omni_slam_slam_eval_node + src/ros/slam_eval.cc + src/ros/stereo_eval.cc + src/ros/odometry_eval.cc + src/ros/reconstruction_eval.cc + src/ros/tracking_eval.cc + src/ros/eval_base.cc + src/omni_slam_slam_eval_node.cc +) + target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} @@ -154,12 +163,22 @@ target_link_libraries(omni_slam_stereo_eval_node PUBLIC omni_slam_eval_lib ${CERES_LIBRARIES} ) +target_link_libraries(omni_slam_slam_eval_node PUBLIC omni_slam_eval_lib + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${HDF5_CXX_LIBRARIES} + ${OpenMP_CXX_FLAGS} + ${PCL_LIBRARIES} + ${CERES_LIBRARIES} +) + target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) target_compile_options(omni_slam_matching_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) target_compile_options(omni_slam_reconstruction_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) target_compile_options(omni_slam_odometry_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) target_compile_options(omni_slam_stereo_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -ggdb") +target_compile_options(omni_slam_slam_eval_node PUBLIC ${OpenMP_CXX_FLAGS}) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -std=c++17") install(TARGETS omni_slam_tracking_eval_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -190,3 +209,9 @@ install(TARGETS omni_slam_stereo_eval_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(TARGETS omni_slam_slam_eval_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch new file mode 100644 index 0000000..2345d5f --- /dev/null +++ b/launch/slam_eval.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + camera_model: '$(arg camera_model)' + camera_parameters: $(arg camera_params) + detector_type: 'GFTT' + detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} + tracker_window_size: 128 + tracker_num_scales: 4 + tracker_checker_epipolar_threshold: 0.008 + tracker_checker_iterations: 1000 + tracker_delta_pixel_error_threshold: 0.0 + tracker_error_threshold: 20.0 + min_features_per_region: 100 + pnp_inlier_threshold: 3.0 + pnp_iterations: 3000 + max_reprojection_error: 5.0 + min_triangulation_angle: 3.0 + bundle_adjustment_max_iterations: 1000 + bundle_adjustment_loss_coefficient: 0.05 + bundle_adjustment_logging: true + bundle_adjustment_num_threads: 20 + stereo_matcher_window_size: 256 + stereo_matcher_num_scales: 5 + stereo_matcher_error_threshold: 20 + stereo_matcher_epipolar_threshold: 0.008 + stereo_tf_t: [2.0, 0.0, 0.0] + stereo_tf_r: [0.0, 0.0, 0.0, 1.0] + + + + + diff --git a/src/data/frame.cc b/src/data/frame.cc index fa4eb6f..2e35b63 100644 --- a/src/data/frame.cc +++ b/src/data/frame.cc @@ -15,7 +15,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix depthImage_(depth_image.clone()), pose_(pose), invPose_(util::TFUtil::InversePoseMatrix(pose)), - stereoPose_(stereo_pose), + stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), cameraModel_(camera_model) { @@ -28,7 +28,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &stereo : id_(lastFrameId_++), image_(image.clone()), stereoImage_(stereo_image.clone()), - stereoPose_(stereo_pose), + stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), cameraModel_(camera_model) { @@ -54,7 +54,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix image_(image.clone()), stereoImage_(stereo_image.clone()), depthImage_(depth_image.clone()), - stereoPose_(stereo_pose), + stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), cameraModel_(camera_model) { @@ -82,7 +82,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, stereoImage_(stereo_image.clone()), pose_(pose), invPose_(util::TFUtil::InversePoseMatrix(pose)), - stereoPose_(stereo_pose), + stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), cameraModel_(camera_model) { diff --git a/src/module/reconstruction_module.cc b/src/module/reconstruction_module.cc index 79a4794..276e342 100644 --- a/src/module/reconstruction_module.cc +++ b/src/module/reconstruction_module.cc @@ -47,7 +47,7 @@ void ReconstructionModule::BundleAdjust(std::vector &landmarks) { bundleAdjuster_->Optimize(landmarks); - for (int i = 0; i < landmarks.size(); i++) + for (int i = 0; i < lastLandmarksSize_; i++) { if (landmarks[i].HasEstimatedPosition()) { diff --git a/src/module/stereo_module.cc b/src/module/stereo_module.cc index aa92baa..d3b63b8 100644 --- a/src/module/stereo_module.cc +++ b/src/module/stereo_module.cc @@ -1,5 +1,7 @@ #include "stereo_module.h" +#include + using namespace std; namespace omni_slam @@ -33,7 +35,8 @@ void StereoModule::Update(data::Frame &frame, std::vector &landm { Matrix framePose = frame.HasEstimatedPose() ? frame.GetEstimatedPose() : frame.GetPose(); double depth = (landmark.GetEstimatedPosition() - framePose.block<3, 1>(0, 3)).norm(); - visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth); + double depthGnd = (landmark.GetGroundTruth() - frame.GetPose().block<3, 1>(0, 3)).norm(); + visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, depthGnd); } } frameNum_++; @@ -55,12 +58,15 @@ void StereoModule::Visualization::Init(cv::Size img_size) curDepth_ = cv::Mat::zeros(img_size, CV_8UC3); } -void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth) +void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd) { + double err = std::abs(depth - depthGnd) / depthGnd; + err = min(err, 1.0); + depth = min(depth, maxDepth_); cv::circle(curMask_, pt1, 3, cv::Scalar(255, 0, 0), -1); cv::circle(curMask_, pt2 + cv::Point2f(curMask_.cols / 2., 0), 3, cv::Scalar(255, 0, 0), -1); cv::line(curMask_, pt1, pt2 + cv::Point2f(curMask_.cols / 2., 0), cv::Scalar(255, 0, 0), 1); - cv::circle(curDepth_, pt1, 2, cv::Scalar(depth / maxDepth_ * 255, depth / maxDepth_ * 255, depth / maxDepth_ * 255)); + cv::circle(curDepth_, pt1, 2, cv::Scalar((depth / maxDepth_ - err) * 255, (depth / maxDepth_ - err) * 255, depth / maxDepth_ * 255)); } void StereoModule::Visualization::Draw(cv::Mat &img, const cv::Mat &stereo_img) diff --git a/src/module/stereo_module.h b/src/module/stereo_module.h index f3c6f19..4aa6794 100644 --- a/src/module/stereo_module.h +++ b/src/module/stereo_module.h @@ -38,7 +38,7 @@ class StereoModule { public: void Init(cv::Size img_size); - void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth); + void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd); void Draw(cv::Mat &img, const cv::Mat &stereo_img); private: diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index 2fa2eab..355d85d 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -36,11 +36,15 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram { if (landmark.IsObservedInFrame(frame.GetID())) { - if (landmark.HasEstimatedPosition()) + if (frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) { xs.push_back(landmark.GetEstimatedPosition()); } - else if (landmark.HasGroundTruth()) + else if (!frame.HasStereoImage() && landmark.HasEstimatedPosition()) + { + xs.push_back(landmark.GetEstimatedPosition()); + } + else if (!frame.HasStereoImage() && landmark.HasGroundTruth()) { xs.push_back(landmark.GetGroundTruth()); } diff --git a/src/omni_slam_slam_eval_node.cc b/src/omni_slam_slam_eval_node.cc new file mode 100644 index 0000000..3db749a --- /dev/null +++ b/src/omni_slam_slam_eval_node.cc @@ -0,0 +1,13 @@ +#include "ros/slam_eval.h" + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "omni_slam_slam_eval_node"); + + omni_slam::ros::SLAMEval slam_eval; + slam_eval.Run(); + + return 0; +} + + diff --git a/src/reconstruction/triangulator.cc b/src/reconstruction/triangulator.cc index 4ba1256..2e260a1 100644 --- a/src/reconstruction/triangulator.cc +++ b/src/reconstruction/triangulator.cc @@ -20,6 +20,10 @@ int Triangulator::Triangulate(std::vector &landmarks) const for (auto it = landmarks.begin(); it < landmarks.end(); it++) { data::Landmark &landmark = *it; + if (landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) + { + continue; + } Vector3d point; if (TriangulateNViews(landmark.GetObservations(), point)) { diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h index d86814f..f654387 100644 --- a/src/ros/odometry_eval.h +++ b/src/ros/odometry_eval.h @@ -27,10 +27,11 @@ class OdometryEval : public virtual TrackingEval virtual void GetResultsData(std::map>> &data); virtual void Visualize(cv_bridge::CvImagePtr &base_img); + void PublishOdometry(); + std::unique_ptr odometryModule_; private: - void PublishOdometry(); ::ros::Publisher odometryPublisher_; ::ros::Publisher odometryGndPublisher_; diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc new file mode 100644 index 0000000..876b8cb --- /dev/null +++ b/src/ros/slam_eval.cc @@ -0,0 +1,63 @@ +#include "slam_eval.h" + +#include "odometry/pnp.h" +#include "optimization/bundle_adjuster.h" +#include "module/tracking_module.h" + +#include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/Path.h" + +using namespace std; + +namespace omni_slam +{ +namespace ros +{ + +SLAMEval::SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) + : OdometryEval(nh, nh_private), ReconstructionEval(nh, nh_private), StereoEval(nh, nh_private) +{ +} + +void SLAMEval::InitPublishers() +{ + OdometryEval::InitPublishers(); + ReconstructionEval::InitPublishers(); + StereoEval::InitPublishers(); +} + +void SLAMEval::ProcessFrame(unique_ptr &&frame) +{ + trackingModule_->Update(frame); + odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back()); + reconstructionModule_->Update(trackingModule_->GetLandmarks()); + trackingModule_->Redetect(); + stereoModule_->Update(*trackingModule_->GetFrames().back(), trackingModule_->GetLandmarks()); +} + +void SLAMEval::Finish() +{ + ReconstructionEval::Finish(); + PublishOdometry(); +} + +void SLAMEval::GetResultsData(std::map>> &data) +{ +} + +void SLAMEval::Visualize(cv_bridge::CvImagePtr &base_img) +{ + visualized_ = false; + ReconstructionEval::Visualize(base_img); + OdometryEval::Visualize(base_img); +} + +void SLAMEval::Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img) +{ + Visualize(base_img); + StereoEval::Visualize(base_img, base_stereo_img); +} + +} +} + diff --git a/src/ros/slam_eval.h b/src/ros/slam_eval.h new file mode 100644 index 0000000..3e43a29 --- /dev/null +++ b/src/ros/slam_eval.h @@ -0,0 +1,34 @@ +#ifndef _SLAM_EVAL_H_ +#define _SLAM_EVAL_H_ + +#include "odometry_eval.h" +#include "reconstruction_eval.h" +#include "stereo_eval.h" +#include +#include + +namespace omni_slam +{ +namespace ros +{ + +class SLAMEval : public OdometryEval, ReconstructionEval, StereoEval +{ +public: + SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private); + SLAMEval() : SLAMEval(::ros::NodeHandle(), ::ros::NodeHandle("~")) {} + +private: + void InitPublishers(); + + void ProcessFrame(std::unique_ptr &&frame); + void Finish(); + void GetResultsData(std::map>> &data); + void Visualize(cv_bridge::CvImagePtr &base_img); + void Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img); +}; + +} +} + +#endif /* _SLAM_EVAL_H_ */ diff --git a/src/ros/stereo_eval.cc b/src/ros/stereo_eval.cc index 230f78f..281bee6 100644 --- a/src/ros/stereo_eval.cc +++ b/src/ros/stereo_eval.cc @@ -50,8 +50,10 @@ void StereoEval::GetResultsData(std::mapimage.clone(); stereoModule_->Visualize(base_img->image, base_stereo_img->image); matchedImagePublisher_.publish(base_img->toImageMsg()); + base_img->image = orig; } } diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index dbb7976..f2ddeff 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -55,11 +55,16 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod template void TrackingEval::InitPublishers() { - EvalBase::InitPublishers(); + if (!pubInitialized_) + { + EvalBase::InitPublishers(); + + string outputTopic; + this->nhp_.param("tracked_image_topic", outputTopic, string("/omni_slam/tracked")); + trackedImagePublisher_ = this->imageTransport_.advertise(outputTopic, 2); - string outputTopic; - this->nhp_.param("tracked_image_topic", outputTopic, string("/omni_slam/tracked")); - trackedImagePublisher_ = this->imageTransport_.advertise(outputTopic, 2); + pubInitialized_ = true; + } } template @@ -88,8 +93,14 @@ void TrackingEval::GetResultsData(std::map void TrackingEval::Visualize(cv_bridge::CvImagePtr &base_img) { - trackingModule_->Visualize(base_img->image); - trackedImagePublisher_.publish(base_img->toImageMsg()); + if (!visualized_) + { + cv::Mat orig = base_img->image.clone(); + trackingModule_->Visualize(base_img->image); + trackedImagePublisher_.publish(base_img->toImageMsg()); + base_img->image = orig; + visualized_ = true; + } } template class TrackingEval; diff --git a/src/ros/tracking_eval.h b/src/ros/tracking_eval.h index d7a8226..e8af811 100644 --- a/src/ros/tracking_eval.h +++ b/src/ros/tracking_eval.h @@ -30,6 +30,9 @@ class TrackingEval : public virtual EvalBase std::unique_ptr trackingModule_; + bool visualized_{false}; + bool pubInitialized_{false}; + private: image_transport::Publisher trackedImagePublisher_; };