From 272ca951c79895807b0e15b3c5505d64a9a56dba Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Fri, 13 Sep 2019 02:39:58 -0400 Subject: [PATCH] working pnp odometry --- CMakeLists.txt | 24 +++++ launch/matching_eval.launch | 2 +- launch/odometry_eval.launch | 34 +++++++ launch/reconstruction_eval.launch | 3 +- launch/tracking_eval.launch | 2 +- src/module/odometry_module.cc | 31 ++++++ src/module/odometry_module.h | 44 +++++++++ src/module/tracking_module.cc | 5 + src/module/tracking_module.h | 1 + src/odometry/lambda_twist.h | 83 ++++++++-------- src/odometry/pnp.cc | 144 +++++++++++++++++++++++++++- src/odometry/pnp.h | 19 +++- src/omni_slam_odometry_eval_node.cc | 11 +++ src/ros/odometry_eval.cc | 92 ++++++++++++++++++ src/ros/odometry_eval.h | 38 ++++++++ src/util/math_util.h | 2 +- 16 files changed, 487 insertions(+), 48 deletions(-) create mode 100644 launch/odometry_eval.launch create mode 100644 src/module/odometry_module.cc create mode 100644 src/module/odometry_module.h create mode 100644 src/omni_slam_odometry_eval_node.cc create mode 100644 src/ros/odometry_eval.cc create mode 100644 src/ros/odometry_eval.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b7beebe..6424128 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,7 @@ add_library(omni_slam_eval_lib src/module/tracking_module.cc src/module/matching_module.cc src/module/reconstruction_module.cc + src/module/odometry_module.cc src/data/frame.cc src/data/feature.cc src/data/landmark.cc @@ -90,6 +91,13 @@ add_executable(omni_slam_reconstruction_eval_node src/omni_slam_reconstruction_eval_node.cc ) +add_executable(omni_slam_odometry_eval_node + src/ros/odometry_eval.cc + src/ros/tracking_eval.cc + src/ros/eval_base.cc + src/omni_slam_odometry_eval_node.cc +) + target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} @@ -117,9 +125,19 @@ target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_l ${CERES_LIBRARIES} ) +target_link_libraries(omni_slam_odometry_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}) set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}) install(TARGETS omni_slam_tracking_eval_node @@ -139,3 +157,9 @@ install(TARGETS omni_slam_reconstruction_eval_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +install(TARGETS omni_slam_odometry_eval_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/launch/matching_eval.launch b/launch/matching_eval.launch index 395ea0b..0678b54 100644 --- a/launch/matching_eval.launch +++ b/launch/matching_eval.launch @@ -15,7 +15,7 @@ - + camera_model: '$(arg camera_model)' diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch new file mode 100644 index 0000000..26916a5 --- /dev/null +++ b/launch/odometry_eval.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + 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_delta_pixel_error_threshold: 3.0 + tracker_error_threshold: 20.0 + min_features_per_region: 10 + pnp_inlier_threshold: 5.0 + pnp_iterations: 1000 + bundle_adjustment_max_iterations: 1000 + bundle_adjustment_logging: true + + + + diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index 7827c91..1ede7fb 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -10,7 +10,8 @@ - + + camera_model: '$(arg camera_model)' diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch index 8ce877e..452b00b 100644 --- a/launch/tracking_eval.launch +++ b/launch/tracking_eval.launch @@ -10,7 +10,7 @@ - + camera_model: '$(arg camera_model)' diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc new file mode 100644 index 0000000..67b022e --- /dev/null +++ b/src/module/odometry_module.cc @@ -0,0 +1,31 @@ +#include "odometry_module.h" + +using namespace std; + +namespace omni_slam +{ +namespace module +{ + +OdometryModule::OdometryModule(std::unique_ptr &pnp) + : pnp_(std::move(pnp)) +{ +} + +OdometryModule::OdometryModule(std::unique_ptr &&pnp) + : OdometryModule(pnp) +{ +} + +void OdometryModule::Update(std::vector &landmarks, data::Frame &frame) +{ + pnp_->Compute(landmarks, frame); +} + +OdometryModule::Stats& OdometryModule::GetStats() +{ + return stats_; +} + +} +} diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h new file mode 100644 index 0000000..65b05a8 --- /dev/null +++ b/src/module/odometry_module.h @@ -0,0 +1,44 @@ +#ifndef _ODOMETRY_MODULE_H_ +#define _ODOMETRY_MODULE_H_ + +#include +#include +#include + +#include "odometry/pnp.h" +#include "data/landmark.h" + +#include +#include + +using namespace Eigen; + +namespace omni_slam +{ +namespace module +{ + +class OdometryModule +{ +public: + struct Stats + { + }; + + OdometryModule(std::unique_ptr &pnp); + OdometryModule(std::unique_ptr &&pnp); + + void Update(std::vector &landmarks, data::Frame &frame); + + Stats& GetStats(); + +private: + std::shared_ptr pnp_; + + Stats stats_; +}; + +} +} + +#endif /* _ODOMETRY_MODULE_H_ */ diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index cab740e..e0eb156 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -135,6 +135,11 @@ std::vector& TrackingModule::GetLandmarks() return landmarks_; } +std::vector>& TrackingModule::GetFrames() +{ + return frames_; +} + TrackingModule::Stats& TrackingModule::GetStats() { return stats_; diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index 78ae27b..be70706 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -34,6 +34,7 @@ class TrackingModule void Redetect(); std::vector& GetLandmarks(); + std::vector>& GetFrames(); Stats& GetStats(); void Visualize(cv::Mat &base_img); diff --git a/src/odometry/lambda_twist.h b/src/odometry/lambda_twist.h index bb132be..6bb3cf2 100644 --- a/src/odometry/lambda_twist.h +++ b/src/odometry/lambda_twist.h @@ -37,7 +37,7 @@ template<> constexpr long double GetNumericLimit() return 1e-15 ; } -template +template T SolveCubic(T b, T c, T d) { T r0; @@ -75,10 +75,10 @@ T SolveCubic(T b, T c, T d) /* Do ITER Newton-Raphson iterations */ /* Break if position of root changes less than 1e.16 */ - T fx,fpx; + T fx, fpx; for (unsigned int cnt = 0; cnt < iterations; ++cnt) { - fx = (((r0 + b) * r0 + c) * r0 + d); + fx = ((r0 + b) * r0 + c) * r0 + d; if ((cnt < 7 || std::abs(fx) > GetNumericLimit())) { @@ -96,11 +96,11 @@ T SolveCubic(T b, T c, T d) } template -void SolveEigenWithKnownZero(Matrix x, Matrix &E, Matrix &L) +void SolveEigenWithKnownZero(Matrix &x, Matrix &E, Matrix &L) { // one eigenvalue is known to be 0. //the known one... - L(2)=0; + L(2) = 0; Matrix v3; v3 << x(3) * x(7) - x(6) * x(4), @@ -129,13 +129,14 @@ void SolveEigenWithKnownZero(Matrix x, Matrix &E, Matrix v1(a1, a2, rnorm); + Matrix v1; + v1 << a1, a2, rnorm; T tmp2 = 1.0 / (e2 * (x(0, 0) + x(1, 1)) + mx0011 - e2 * e2 + x01_squared); T a21 = -(e2 * x(0, 2) + prec_0) * tmp2; @@ -143,15 +144,16 @@ void SolveEigenWithKnownZero(Matrix x, Matrix &E, Matrix v2(a21, a22, rnorm2); + Matrix v2; + v2 << a21, a22, rnorm2; - E = Matrix(v1[0], v2[0], v3[0], v1[1], v2[1], v3[1], v1[2], v2[2], v3[2]); + E << v1[0], v2[0], v3[0], v1[1], v2[1], v3[1], v1[2], v2[2], v3[2]; } template -void GaussNewtonRefineL(Matrix &L, T a12, T a13, T a23, T b12, T b13, T b23 ){ - - for(int i=0; i < iterations; ++i) +void GaussNewtonRefineL(Matrix &L, T a12, T a13, T a23, T b12, T b13, T b23) +{ + for (int i = 0; i < iterations; ++i) { T l1 = L(0); T l2 = L(1); @@ -197,7 +199,7 @@ void GaussNewtonRefineL(Matrix &L, T a12, T a13, T a23, T b12, T b13, T T r11 = l1 * l1 + l2 * l2 + b12 * l1 * l2 - a12; T r12 = l1 * l1 + l3 * l3 + b13 * l1 * l3 - a13; T r13 = l2 * l2 + l3 * l3 + b23 * l2 * l3 - a23; - if(std::abs(r11) + std::abs(r12) + std::abs(r13) > std::abs(r1) + std::abs(r2) + std::abs(r3)) + if (std::abs(r11) + std::abs(r12) + std::abs(r13) > std::abs(r1) + std::abs(r2) + std::abs(r3)) { break; } @@ -211,7 +213,7 @@ void GaussNewtonRefineL(Matrix &L, T a12, T a13, T a23, T b12, T b13, T } template -int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix x1, Matrix x2, Matrix x3, std::vector>& Rs, std::vector>& Ts) +int P3P(Matrix y1, Matrix y2, Matrix y3, Matrix x1, Matrix x2, Matrix x3, std::vector>& Rs, std::vector>& Ts) { y1.normalize(); y2.normalize(); @@ -240,7 +242,7 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix y1, Matrix y2, Matrix y3, Matrix y1, Matrix y2, Matrix y3, Matrix y1, Matrix y2, Matrix y3, Matrix= 0) + if (b * b - 4.0 * c >= 0) { T tau1, tau2; util::MathUtil::Roots(b, c, tau1, tau2); - if(tau1 > 0) + if (tau1 > 0) { T tau = tau1; T d = a23 / (tau * (b23 + tau) + T(1.0)); @@ -304,13 +306,13 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix= 0) + if (l1 >= 0) { Ls[valid] << l1, l2, l3; ++valid; } } - if(tau2 > 0) + if (tau2 > 0) { T tau = tau2; T d = a23 / (tau * (b23 + tau) + T(1.0)); @@ -327,8 +329,8 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix y1, Matrix y2, Matrix y3, Matrix= 0){ + if (b * b - 4.0 * c >= 0) + { T tau1, tau2; util::MathUtil::Roots(b, c, tau1, tau2); @@ -344,14 +347,14 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix 0) + if (d > 0) { T l2 = std::sqrt(d); T l3 = tau * l2; T l1 = w0 * l2 + w1 * l3; - if(l1 >= 0) + if (l1 >= 0) { Ls[valid] << l1, l2, l3; ++valid; @@ -388,20 +391,16 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix yd2; Matrix yd1xd2; Matrix X; - X << d12(0),d13(0),d12xd13(0), - d12(1),d13(1),d12xd13(1), - d12(2),d13(2),d12xd13(2); - X=X.inverse(); - - Rs.clear(); - Rs.resize(4); - Ts.clear(); - Ts.resize(4); + X << d12(0), d13(0), d12xd13(0), + d12(1), d13(1), d12xd13(1), + d12(2), d13(2), d12xd13(2); + Matrix Xinv = X.inverse(); + for(int i = 0; i < valid; ++i) { - ry1 = y1 * Ls(i)(0); - ry2 = y2 * Ls(i)(1); - ry3 = y3 * Ls(i)(2); + ry1 = y1 * Ls[i](0); + ry2 = y2 * Ls[i](1); + ry3 = y3 * Ls[i](2); yd1 = ry1 - ry2; yd2 = ry1 - ry3; @@ -412,7 +411,7 @@ int P3P( Matrix y1, Matrix y2, Matrix y3, Matrix +#include + namespace omni_slam { namespace odometry { -PNP::PNP() +PNP::PNP(int ransac_iterations, double reprojection_threshold) + : ransacIterations_(ransac_iterations), + reprojThreshold_(reprojection_threshold) +{ +} + +int PNP::Compute(const std::vector &landmarks, data::Frame &frame) const +{ + std::vector xs; + std::vector yns; + std::vector ys; + for (const data::Landmark &landmark : landmarks) + { + if (landmark.IsObservedInFrame(frame.GetID())) + { + if (landmark.HasEstimatedPosition()) + { + xs.push_back(landmark.GetEstimatedPosition()); + } + else if (landmark.HasGroundTruth()) + { + xs.push_back(landmark.GetGroundTruth()); + } + else + { + continue; + } + const data::Feature *feat = landmark.GetObservationByFrameID(frame.GetID()); + Vector2d pix; + pix << feat->GetKeypoint().pt.x, feat->GetKeypoint().pt.y; + yns.push_back(pix); + ys.push_back(feat->GetBearing()); + } + } + Matrix pose; + int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose); + frame.SetEstimatedInversePose(pose); + 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; + for (int i = 0; i < ransacIterations_; i++) + { + std::random_device rd; + std::mt19937 eng(rd()); + std::uniform_int_distribution<> distr(0, xs.size() - 1); + std::set randset; + while (randset.size() < 4) + { + randset.insert(distr(eng)); + } + std::vector indices; + std::copy(randset.begin(), randset.end(), std::back_inserter(indices)); + + Matrix iterPose; + double err = P4P(xs, ys, yns, indices, camera_model, iterPose); + if (std::isnan(err)) + { + continue; + } + + int inliers = CountInliers(xs, yns, iterPose, camera_model); + if (inliers > maxInliers) + { + maxInliers = inliers; + pose = iterPose; + } + } + return maxInliers; +} + +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); + std::vector Ts(4); + + int valid = LambdaTwist::P3P(ys[indices[0]], ys[indices[1]], ys[indices[2]], xs[indices[0]], xs[indices[1]], xs[indices[2]], Rs, Ts); + + const Vector2d &y = yns[indices[3]]; + const Vector3d &x = xs[indices[3]]; + + if (valid == 0) + { + return std::numeric_limits::quiet_NaN(); + } + + double e0 = std::numeric_limits::max(); + + for (int v = 0; v < valid; ++v) + { + Matrix tmp; + tmp.block<3, 3>(0, 0) = Rs[v]; + tmp.block<3, 1>(0, 3) = Ts[v]; + + Vector2d xr; + if (!camera_model.ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(tmp, x)), xr)) + { + continue; + } + double e = (xr - y).array().abs().sum(); + if (std::isnan(e)) + { + continue; + } + if (e < e0 && ((tmp - tmp).array() == (tmp - tmp).array()).all() && ((tmp.array() == tmp.array()).all())) + { + pose = tmp; + e0 = e; + } + } + + double error = 0; + for (int i = 0; i < 4; i++) + { + Vector2d xr; + camera_model.ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, xs[indices[i]])), xr); + double e = (xr - yns[indices[i]]).squaredNorm(); + error += e; + } + return error; +} + +int PNP::CountInliers(const std::vector &xs, const std::vector &yns, const Matrix &pose, const camera::CameraModel<> &camera_model) const { + int inliers = 0; + double thresh = reprojThreshold_ * reprojThreshold_; + for (int i = 0; i < xs.size(); i++) + { + Vector2d xr; + if (!camera_model.ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, xs[i])), xr)) + { + continue; + } + double err = (xr - yns[i]).squaredNorm(); + inliers += (err < thresh) ? 1 : 0; + } + return inliers; } } diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h index 48bc96d..2282ecf 100644 --- a/src/odometry/pnp.h +++ b/src/odometry/pnp.h @@ -1,6 +1,14 @@ #ifndef _PNP_H_ #define _PNP_H_ +#include +#include "data/landmark.h" +#include "data/frame.h" +#include "camera/camera_model.h" +#include + +using namespace Eigen; + namespace omni_slam { namespace odometry @@ -9,7 +17,16 @@ namespace odometry class PNP { public: - PNP(); + PNP(int ransac_iterations, double reprojection_threshold); + int Compute(const std::vector &landmarks, data::Frame &frame) const; + +private: + int RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, 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; + + int ransacIterations_; + double reprojThreshold_; }; } diff --git a/src/omni_slam_odometry_eval_node.cc b/src/omni_slam_odometry_eval_node.cc new file mode 100644 index 0000000..5f3c95f --- /dev/null +++ b/src/omni_slam_odometry_eval_node.cc @@ -0,0 +1,11 @@ +#include "ros/odometry_eval.h" + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "omni_slam_odometry_eval_node"); + + omni_slam::ros::OdometryEval odometry_eval; + odometry_eval.Run(); + + return 0; +} diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc new file mode 100644 index 0000000..ba7a91d --- /dev/null +++ b/src/ros/odometry_eval.cc @@ -0,0 +1,92 @@ +#include "odometry_eval.h" + +#include "odometry/pnp.h" +#include "module/tracking_module.h" + +#include "geometry_msgs/PoseStamped.h" + +using namespace std; + +namespace omni_slam +{ +namespace ros +{ + +OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) + : TrackingEval(nh, nh_private) +{ + double reprojThresh; + int iterations; + int baMaxIter; + bool logCeres; + + 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_logging", logCeres, false); + + unique_ptr pnp(new odometry::PNP(iterations, reprojThresh)); + + odometryModule_.reset(new module::OdometryModule(pnp)); +} + +void OdometryEval::InitPublishers() +{ + TrackingEval::InitPublishers(); + string outputTopic; + string outputGndTopic; + nhp_.param("odometry_estimate_topic", outputTopic, string("/omni_slam/odometry")); + nhp_.param("odometry_ground_truth_topic", outputGndTopic, string("/omni_slam/odometry_truth")); + odometryPublisher_ = nh_.advertise(outputTopic, 2); + odometryGndPublisher_ = nh_.advertise(outputGndTopic, 2); +} + +void OdometryEval::ProcessFrame(unique_ptr &&frame) +{ + trackingModule_->Update(frame); + odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back()); + trackingModule_->Redetect(); +} + +void OdometryEval::Finish() +{ +} + +void OdometryEval::GetResultsData(std::map>> &data) +{ + module::OdometryModule::Stats &stats = odometryModule_->GetStats(); +} + +void OdometryEval::Visualize(cv_bridge::CvImagePtr &base_img) +{ + TrackingEval::Visualize(base_img); + geometry_msgs::PoseStamped poseMsg; + poseMsg.header.stamp = ::ros::Time::now(); + poseMsg.header.frame_id = "map"; + if (trackingModule_->GetFrames().back()->HasEstimatedPose()) + { + const Matrix &pose = trackingModule_->GetFrames().back()->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(); + odometryPublisher_.publish(poseMsg); + } + const Matrix &poseGnd = trackingModule_->GetFrames().back()->GetPose(); + poseMsg.pose.position.x = poseGnd(0, 3); + poseMsg.pose.position.y = poseGnd(1, 3); + poseMsg.pose.position.z = poseGnd(2, 3); + Quaterniond quatGnd(poseGnd.block<3, 3>(0, 0)); + poseMsg.pose.orientation.x = quatGnd.normalized().x(); + poseMsg.pose.orientation.y = quatGnd.normalized().y(); + poseMsg.pose.orientation.z = quatGnd.normalized().z(); + poseMsg.pose.orientation.w = quatGnd.normalized().w(); + odometryGndPublisher_.publish(poseMsg); +} + +} +} diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h new file mode 100644 index 0000000..2de3e2d --- /dev/null +++ b/src/ros/odometry_eval.h @@ -0,0 +1,38 @@ +#ifndef _ODOMETRY_EVAL_H_ +#define _ODOMETRY_EVAL_H_ + +#include "tracking_eval.h" +#include +#include + +#include "module/odometry_module.h" + +namespace omni_slam +{ +namespace ros +{ + +class OdometryEval : public TrackingEval +{ +public: + OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private); + OdometryEval() : OdometryEval(::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); + + std::unique_ptr odometryModule_; + + ::ros::Publisher odometryPublisher_; + ::ros::Publisher odometryGndPublisher_; +}; + +} +} + +#endif /* _ODOMETRY_EVAL_H_ */ diff --git a/src/util/math_util.h b/src/util/math_util.h index 8c49265..83c5ea0 100644 --- a/src/util/math_util.h +++ b/src/util/math_util.h @@ -37,7 +37,7 @@ inline T FastAtan2(T y, T x) } template -inline bool Roots(T b, T c,T &r1, T &r2) +inline bool Roots(T b, T c, T &r1, T &r2) { T v = b * b - 4.0 * c; if (v < 0.)