Skip to content
Snippets Groups Projects
Commit 4187aa68 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

New gtest for vision_utils.h

parent 78dacba9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithms/opticalflowpyrlk/alg_opticalflowpyrlk.cpp
+ 2
− 7
View file @ 4187aa68
......@@ -69,17 +69,14 @@ void AlgorithmOPTFLOWPYRLK::detectNewFeatures(FramePtr& _frame, const DetectorBa
// Keep only new features
KeyPointVector tracked_kps = _frame->getKeyPoints();
std::vector<int> kps_idxs;
KeyPointVector kps_new = differenceKPVec(tracked_kps, kps, kps_idxs);
desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
kps_new = differenceKPVec(tracked_kps, kps, kps_idxs);
}
else
{
// Get full frame detections and descriptors
// Get full frame detections
kps_new = _det_ptr->detect(img_gray);
desc_new = cv::Mat(kps_new.size(), 1, CV_32F); // Not used in this context.
}
_frame->setKeyPoints(kps_new);
_frame->setDescriptors(desc_new);
if (params_ptr_->draw_results)
{
......@@ -128,9 +125,7 @@ void AlgorithmOPTFLOWPYRLK::trackFrame(const FramePtr& _frame_old, FramePtr& _tr
}
}
// Update
cv::Mat desc_new = cv::Mat(good_kpts.size(), 1, CV_32F); // Not used in this context.
_tracked_frame->setKeyPoints(good_kpts);
_tracked_frame->setDescriptors(desc_new);
// If not enough tracked features, add new ones
if (_tracked_frame->getNumFeatures() < params_ptr_->min_feat_track)
......
src/common_class/frame.h
+ 0
− 1
View file @ 4187aa68
......@@ -172,7 +172,6 @@ inline void Frame::addDescriptors(const cv::Mat& _desc)
inline int Frame::getNumFeatures(void)
{
assert( kps_.size() == desc_.rows && "The Number of descriptors must be equal to number of keypoints");
return kps_.size();
}
......
src/test/gtest_vision_utils.cpp
+ 111
− 0
View file @ 4187aa68
......@@ -109,6 +109,117 @@ TEST(VisionUtils, intersecKPVec){
ASSERT_EQ(res[1].pt, kp3.pt);
}
TEST(VisionUtils, unionPVec){
PointVector v1;
v1.push_back(p1);
v1.push_back(p3);
PointVector v2;
v2.push_back(p2);
v2.push_back(p4);
PointVector res = vision_utils::unionPVec(v1, v2);
ASSERT_EQ(res.size(), 4);
ASSERT_EQ(res[0], p1);
ASSERT_EQ(res[1], p2);
ASSERT_EQ(res[2], p3);
ASSERT_EQ(res[3], p4);
}
TEST(VisionUtils, unionKPVec){
KeyPointVector v1;
v1.push_back(kp1);
v1.push_back(kp3);
KeyPointVector v2;
v2.push_back(kp2);
v2.push_back(kp4);
KeyPointVector res = vision_utils::unionKPVec(v1, v2);
ASSERT_EQ(res.size(), 4);
ASSERT_EQ(res[0].pt, kp1.pt);
ASSERT_EQ(res[1].pt, kp2.pt);
ASSERT_EQ(res[2].pt, kp3.pt);
ASSERT_EQ(res[3].pt, kp4.pt);
}
TEST(VisionUtils, inflateRoi)
{
cv::Rect roi(0,0,100,100);
int inflation_rate = 10;
vision_utils::inflateRoi(inflation_rate, roi);
ASSERT_EQ(roi.x, -10);
ASSERT_EQ(roi.y, -10);
ASSERT_EQ(roi.width, 120);
ASSERT_EQ(roi.height, 120);
}
TEST(VisionUtils, trimRoi)
{
cv::Rect roi(0,0,100,100);
int inflation_rate = 10;
vision_utils::inflateRoi(inflation_rate, roi);
vision_utils::trimRoi(100,100, roi);
ASSERT_EQ(roi.x, 0);
ASSERT_EQ(roi.y, 0);
ASSERT_EQ(roi.width, 100);
ASSERT_EQ(roi.height, 100);
}
TEST(VisionUtils, adaptROI)
{
cv::Mat img(100,100,CV_32F);
cv::Rect roi(0,0,100,100);
int inflation_rate = 10;
cv::Mat img_roi;
vision_utils::adaptRoi(inflation_rate, img, roi, img_roi);
ASSERT_EQ(img.rows, img_roi.rows);
ASSERT_EQ(img.cols, img_roi.cols);
}
TEST(VisionUtils, retainBest)
{
KeyPointVector v1;
v1.push_back(kp1);
v1.push_back(kp2);
float response_best = 10.0;
cv::KeyPoint kpbest(p3,CV_32F,-1,response_best);
v1.push_back(kpbest);
v1.push_back(kp4);
vision_utils::retainBest(v1,1);
ASSERT_EQ(v1.size(), 1);
ASSERT_EQ(v1[0].pt, p3);
}
TEST(VisionUtils, projectPoints)
{
Eigen::MatrixXf points3D(2,3);
Eigen::Vector3f point3D;
point3D[0] = 2.0;
point3D[1] = 5.0;
point3D[2] = 6.0;
points3D.row(0)= point3D;
point3D[0] = 4.0;
point3D[1] = 2.0;
point3D[2] = 1.0;
points3D.row(1)= point3D;
Eigen::Vector3f cam_ext_rot_mat = Eigen::Vector3f::Zero();
Eigen::Vector3f cam_ext_trans_mat = Eigen::Vector3f::Ones();
Eigen::Matrix3f cam_intr_mat;
cam_intr_mat = Eigen::Matrix3f::Identity();
cam_intr_mat(0,2)=2;
cam_intr_mat(1,2)=2;
Eigen::VectorXf dist_coef(5);
dist_coef << 0,0,0,0,0;
Eigen::MatrixXf points2D = vision_utils::projectPoints(points3D, cam_ext_rot_mat, cam_ext_trans_mat, cam_intr_mat, dist_coef);
ASSERT_EQ(points2D.rows(), points3D.rows());
ASSERT_TRUE(points2D(0,0)-2.42857 < 1e-5);
ASSERT_TRUE(points2D(0,1)-2.85714 < 1e-5);
ASSERT_TRUE(points2D(1,0)-4.5 < 1e-5);
ASSERT_TRUE(points2D(1,1)-3.5 < 1e-5);
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
......
src/vision_utils.cpp
+ 31
− 94
View file @ 4187aa68
......@@ -114,99 +114,6 @@ KeyPointVector unionKPVec(KeyPointVector& v1, KeyPointVector& v2)
return v3;
};
void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to)
{
_kpv_to.clear();
for (auto kp : _kpv_from)
_kpv_to.push_back(kp);
};
PointVector KPToP(const KeyPointVector& _kp_vec)
{
PointVector pt_vec;
for (auto kp : _kp_vec)
pt_vec.push_back(kp.pt);
return pt_vec;
};
void whoHasMoved(const KeyPointVector& _kp_vec_1, const KeyPointVector& _kp_vec_2, KeyPointVector& _common_kp_vec, KeyPointVector& _exclusive_in_kp_vec_2)
{
_exclusive_in_kp_vec_2.clear();
_common_kp_vec.clear();
for (auto kp2 : _kp_vec_2)
{
bool existing = false;
for (auto kp1 : _kp_vec_1)
{
cv::Point2f p1 = kp2.pt;
cv::Point2f p2 = kp1.pt;
if (p1.x-p2.x < 1.0 || p1.y-p2.y < 1.0)
{
existing = true;
break;
}
}
if (!existing) // New feature
_exclusive_in_kp_vec_2.push_back(kp2);
else
_common_kp_vec.push_back(kp2);
}
}
void whoHasMoved(const PointVector& _pt_vec_1, const PointVector& _pt_vec_2, PointVector& _common_pt_vec, PointVector& _exclusive_in_pt_vec_2)
{
_exclusive_in_pt_vec_2.clear();
_common_pt_vec.clear();
for (auto p2 : _pt_vec_2)
{
bool existing = false;
for (auto p1 : _pt_vec_1)
{
if (p1.x-p2.x < 1.0 || p1.y-p2.y < 1.0)
{
existing = true;
break;
}
}
if (!existing) // New feature
_exclusive_in_pt_vec_2.push_back(p2);
else
_common_pt_vec.push_back(p2);
}
}
void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius, const cv::Scalar& _color, const int& _thickness)
{
for (auto kp : _kp_vec)
cv::circle(_image, kp.pt, _radius, _color, _thickness);
};
void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color)
{
for (auto kl : _kl_vec)
cv::line(_image, kl.getStartPoint(), kl.getEndPoint(), _color, 3);
};
void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness, const double& _alpha)
{
for (auto roi_it : _roi_vec)
drawRoi(_image, roi_it, _window_name, _color, _thickness, _alpha);
};
void drawRoi(cv::Mat _image, const cv::Rect& _roi, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness, const double& _alpha)
{
if (_alpha > 0.0)
{
assert(_thickness!=-1);
cv::Mat color(_roi.size(), CV_8UC3, _color);
cv::Mat roi = _image(_roi);
cv::addWeighted(color, _alpha, roi, 1.0 - _alpha , 0.0, roi);
}
cv::rectangle(_image, _roi, _color, _thickness, 8, 0);
};
void trimRoi(const int& _img_width, const int& _img_height, cv::Rect& _roi)
{
if(_roi.x < 0)
......@@ -256,7 +163,7 @@ void retainBest(KeyPointVector& _kpts, int n)
Eigen::MatrixXf projectPoints(const Eigen::MatrixXf& _points3D, const Eigen::Vector3f& _rot_mat, const Eigen::Vector3f& _trans_mat, const Eigen::MatrixXf& _cam_mat, const Eigen::VectorXf& _dist_coef)
{
// Check row-wise points
assert(_points3D.cols()!=3 && "[Vision Utils]: The 3D points are expected to be stored row-wise");
assert(_points3D.cols()==3 && "[Vision Utils]: The 3D points are expected to be stored row-wise");
cv::Mat cv_3Dpoints;
cv::eigen2cv(_points3D, cv_3Dpoints);
......@@ -310,5 +217,35 @@ std::string readYamlType(const std::string& _filename_dot_yaml, const std::strin
}
#endif
void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius, const cv::Scalar& _color, const int& _thickness)
{
for (auto kp : _kp_vec)
cv::circle(_image, kp.pt, _radius, _color, _thickness);
};
void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color)
{
for (auto kl : _kl_vec)
cv::line(_image, kl.getStartPoint(), kl.getEndPoint(), _color, 3);
};
void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness, const double& _alpha)
{
for (auto roi_it : _roi_vec)
drawRoi(_image, roi_it, _window_name, _color, _thickness, _alpha);
};
void drawRoi(cv::Mat _image, const cv::Rect& _roi, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness, const double& _alpha)
{
if (_alpha > 0.0)
{
assert(_thickness!=-1);
cv::Mat color(_roi.size(), CV_8UC3, _color);
cv::Mat roi = _image(_roi);
cv::addWeighted(color, _alpha, roi, 1.0 - _alpha , 0.0, roi);
}
cv::rectangle(_image, _roi, _color, _thickness, 8, 0);
};
} /* namespace vision_utils */
src/vision_utils.h
+ 9
− 18
View file @ 4187aa68
......@@ -142,24 +142,6 @@ KeyPointVector intersecKPVec(KeyPointVector& v1, KeyPointVector& v2);
PointVector unionPVec(PointVector& v1, PointVector& v2);
KeyPointVector unionKPVec(KeyPointVector& v1, KeyPointVector& v2);
void copyKPVector(const KeyPointVector& _kpv_from, KeyPointVector& _kpv_to);
PointVector KPToP(const KeyPointVector& _kp_vec);
void whoHasMoved(const KeyPointVector& _kpts1, const KeyPointVector& _kpts2, KeyPointVector& _common_kpts, KeyPointVector& _new_in_kpts2);
void whoHasMoved(const PointVector& _pts1, const PointVector& _pts2, PointVector& _common_pts, PointVector& _new_in_pts2);
void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color = cv::Scalar(0, 0, 255), const int& _thickness=-1);
void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) );
void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const std::string& _window_name, const cv::Scalar& _color = cv::Scalar(128, 128, 255), const int& _thickness = 1, const double& _alpha = 0.0);
void drawRoi(cv::Mat _image, const cv::Rect& _roi, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness = 1, const double& _alpha = 0.0);
void trimRoi(const int& _img_width, const int& _img_height, cv::Rect& _roi);
void adaptRoi(const int& _inflation_rate, const cv::Mat& _image, cv::Rect& _roi, cv::Mat& _image_roi);
template <typename T>
void inflateRoi(const T& _inflation_rate, cv::Rect& _roi)
{
......@@ -169,6 +151,10 @@ void inflateRoi(const T& _inflation_rate, cv::Rect& _roi)
_roi.height = _roi.height + 2*_inflation_rate;
}
void trimRoi(const int& _img_width, const int& _img_height, cv::Rect& _roi);
void adaptRoi(const int& _inflation_rate, const cv::Mat& _image, cv::Rect& _roi, cv::Mat& _image_roi);
template <typename T1, typename T2 >
cv::Rect setRoi(const T1& _center_x, const T1& _center_y, const T2& _roi_width, const T2& _roi_heigth)
{
......@@ -184,6 +170,11 @@ void retainBest(KeyPointVector& _kpts, int n);
Eigen::MatrixXf projectPoints(const Eigen::MatrixXf& _points3D, const Eigen::Vector3f& _rot_mat, const Eigen::Vector3f& _trans_mat, const Eigen::MatrixXf& _cam_mat, const Eigen::VectorXf& _dist_coef);
void drawKeyPoints(cv::Mat& _image, const KeyPointVector& _kp_vec, const int& _radius=5, const cv::Scalar& _color = cv::Scalar(0, 0, 255), const int& _thickness=-1);
void drawKeyLines(cv::Mat& _image, const KeyLineVector& _kl_vec, const cv::Scalar& _color = cv::Scalar(128, 128, 255) );
void drawRoi(cv::Mat _image, const RectVector& _roi_vec, const std::string& _window_name, const cv::Scalar& _color = cv::Scalar(128, 128, 255), const int& _thickness = 1, const double& _alpha = 0.0);
void drawRoi(cv::Mat _image, const cv::Rect& _roi, const std::string& _window_name, const cv::Scalar& _color, const int& _thickness = 1, const double& _alpha = 0.0);
} /* namespace vision_utils */
#endif
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment