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.)