pnp forward

CMakeLists.txt

@@ -15,21 +15,18 @@ set(CMAKE_BUILD_TYPE "RELEASE")
 
 include_directories(
 		/usr/include
+		PnP
+		StripCenter
 		{EIGEN3_INCLUDE_DIR}
   ${OpenCV_INCLUDE_DIRS}
 )
 link_directories("/usr/local/lib")
 
 
-add_executable(triangle_measure ./main.cpp ./Steger.cpp)
-target_link_libraries(triangle_measure ${OpenCV_LIBS})
 
-add_executable(caliber ./camera_calib/caliber.cpp)
-target_link_libraries(caliber ${OpenCV_LIBS})
 
-
-add_executable( pose_estimation_3d2d PnP/pose_estimation_3d2d.cpp )
-target_link_libraries( pose_estimation_3d2d
+add_executable( triangle_measure main.cpp StripCenter/Steger.cpp PnP/pnp.cpp)
+target_link_libraries( triangle_measure
 		${OpenCV_LIBS}
 		)
 

PnP/TimeRecord.cpp

@@ -0,0 +1,75 @@
+//
+// Created by zx on 22-11-3.
+//
+
+#include "TimeRecord.h"
+#include <iostream>
+std::map<std::string, TimeRecord::Record> TimeRecord::records_=std::map<std::string, TimeRecord::Record>();
+
+
+template <class F>
+void TimeRecord::Evaluate(F&& func, const std::string& func_name)
+{
+    auto t1 = std::chrono::high_resolution_clock::now();
+    std::forward<F>(func)();
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
+
+    if (records_.find(func_name) != records_.end()) {
+        records_[func_name].time_usage_in_ms_.emplace_back(time_used);
+    } else {
+        records_.insert({func_name, Record(func_name, time_used)});
+    }
+}
+void TimeRecord::PrintAll()
+{
+    std::cout <<">> ===== Printing run time ====="<<std::endl;
+    for (const auto& r : records_) {
+        std::cout<< "> [ " << r.first << " ] average time usage: "
+                  << std::accumulate(r.second.time_usage_in_ms_.begin(), r.second.time_usage_in_ms_.end(), 0.0) /
+                          double(r.second.time_usage_in_ms_.size())
+                  << " ms , called times: " << r.second.time_usage_in_ms_.size()<<std::endl;
+    }
+    std::cout << ">>> ===== Printing run time end ====="<<std::endl;
+}
+void TimeRecord::DumpIntoFile(const std::string& file_name)
+{
+    std::ofstream ofs(file_name, std::ios::out);
+    if (!ofs.is_open()) {
+        std::cout << "Failed to open file: " << file_name<<std::endl;
+        return;
+    } else {
+        std::cout << "Dump Time Records into file: " << file_name<<std::endl;
+    }
+
+    size_t max_length = 0;
+    for (const auto& iter : records_) {
+        ofs << iter.first << ", ";
+        if (iter.second.time_usage_in_ms_.size() > max_length) {
+            max_length = iter.second.time_usage_in_ms_.size();
+        }
+    }
+    ofs << std::endl;
+
+    for (size_t i = 0; i < max_length; ++i) {
+        for (const auto& iter : records_) {
+            if (i < iter.second.time_usage_in_ms_.size()) {
+                ofs << iter.second.time_usage_in_ms_[i] << ",";
+            } else {
+                ofs << ",";
+            }
+        }
+        ofs << std::endl;
+    }
+    ofs.close();
+}
+double TimeRecord::GetMeanTime(const std::string& func_name)
+{
+    if (records_.find(func_name) == records_.end()) {
+        return 0.0;
+    }
+
+    auto r = records_[func_name];
+    return std::accumulate(r.time_usage_in_ms_.begin(), r.time_usage_in_ms_.end(), 0.0) /
+            double(r.time_usage_in_ms_.size());
+}

PnP/TimeRecord.h

@@ -0,0 +1,40 @@
+//
+// Created by zx on 22-11-3.
+//
+
+#ifndef SRC_LIO_LIVOX_SRC_UTILS_TIMERECORD_H_
+#define SRC_LIO_LIVOX_SRC_UTILS_TIMERECORD_H_
+
+#include <chrono>
+#include <fstream>
+#include <map>
+#include <numeric>
+#include <string>
+#include <vector>
+
+class TimeRecord
+{
+ public:
+    struct Record {
+        Record() = default;
+        Record(const std::string& name, double time_usage) {
+            func_name_ = name;
+            time_usage_in_ms_.emplace_back(time_usage);
+        }
+        std::string func_name_;
+        std::vector<double> time_usage_in_ms_;
+    };
+
+    template <class F>
+    static void Evaluate(F&& func, const std::string& func_name);
+    static void PrintAll();
+    static void DumpIntoFile(const std::string& file_name);
+    static double GetMeanTime(const std::string& func_name);
+ private:
+    static std::map<std::string, Record> records_;
+
+
+};
+
+
+#endif //SRC_LIO_LIVOX_SRC_UTILS_TIMERECORD_H_

PnP/pnp.cpp

@@ -3,10 +3,11 @@
 //
 
 #include "pnp.h"
+#include <chrono>
 
-Point2d pixel2cam ( const Point2d& p, const Mat& K )
+cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K )
 {
-    return Point2d
+    return cv::Point2d
             (
                     ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
                     ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
@@ -22,40 +23,40 @@ P3d2d::~P3d2d()
 
 }
 
-void P3d2d::Calibrate(std::vector<cv::Point3f> pts_3d,std::vector<cv::Point2f> pts_2d)
+void P3d2d::Calibrate(std::vector<cv::Point3d> pts_3d,std::vector<cv::Point2d> pts_2d)
 {
-    Mat r;
-    solvePnP ( pts_3d, pts_2d, K_, Mat(), r, t_, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
+    cv::Mat r;
+    solvePnP ( pts_3d, pts_2d, K_, cv::Mat(), r, t_, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
     cv::Rodrigues ( r, R_ ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
 }
 
-std::vector<cv::Point3f> P3d2d::excute(std::vector<cv::Point2f> pts_2d)
+std::vector<cv::Point3d> P3d2d::execute(std::vector<cv::Point2d> pts_2d)
 {
 
-    Mat RINV=R.inv();
+    cv::Mat TR=cv::Mat::zeros(3,3,R_.type());
 
-    for(int i=0;i<pts_3d.size();++i)
-    {
-        Point2d p1 = pixel2cam ( pts_2d[i], K );
-
-        //先求该点在相机中的d值，依据条件，该点世界坐标y=0
-        //旋转矩阵 R*([x,0,z]+t_)=(p1.x*d, p1.y*d, d)
-
-        Point3f pose3d ( Point3f ( p1.x*distance, p1.y*distance, distance ) );
+    std::vector<cv::Point3d> world_points;
 
 
+    for(int i=0;i<pts_2d.size();i++)
+    {
+        cv::Point2d pt = pixel2cam(pts_2d[i], K_);
+        double a = pt.x;
+        double b = pt.y;
 
-        Mat pose3d_m=Mat::zeros(3,1,R.type());
+        TR.at<double>(0, 0) = a * R_.at<double>(0, 0) + b * R_.at<double>(0, 1) + R_.at<double>(0, 2);
+        TR.at<double>(0, 1) = -1;
 
-        pose3d_m.at<double>(0,0)=pose3d.x-t.at<double>(0,0);
-        pose3d_m.at<double>(1,0)=pose3d.y-t.at<double>(1,0);
-        pose3d_m.at<double>(2,0)=pose3d.z-t.at<double>(2,0);
+        TR.at<double>(1, 0) = a * R_.at<double>(1, 0) + b * R_.at<double>(1, 1) + R_.at<double>(1, 2);
+        TR.at<double>(2, 0) = a * R_.at<double>(2, 0) + b * R_.at<double>(2, 1) + R_.at<double>(2, 2);
+        TR.at<double>(2, 2) = -1;
 
-        Mat o_pose=RINV*pose3d_m;
+        cv::Mat T = -t_;
+        cv::Mat dxz = TR.inv() * T;
 
-        printf(" cal :%f, %f, %f   org:%f, %f, %f\n",o_pose.at<double>(0,0),o_pose.at<double>(1,0),o_pose.at<double>(2,0),
-               pts_3d[i].x,pts_3d[i].y,pts_3d[i].z);
+        world_points.push_back(cv::Point3d(dxz.at<double>(1,0),0,dxz.at<double>(2,0)));
 
     }
+    return world_points;
 
 }

PnP/pnp.h

@@ -7,7 +7,7 @@
 
 #include <opencv2/opencv.hpp>
 
-Point2d pixel2cam ( const Point2d& p, const Mat& K );
+cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K );
 
 class P3d2d
 {
@@ -15,12 +15,12 @@ class P3d2d
     P3d2d(cv::Mat K);
     ~P3d2d();
 
-    void Calibrate(std::vector<cv::Point3f> pts_3d,std::vector<cv::Point2f> pts_2d)
+    void Calibrate(std::vector<cv::Point3d> pts_3d,std::vector<cv::Point2d> pts_2d);
 
-    std::vector<cv::Point3f> excute(std::vector<cv::Point2f> pts_2d);
+    std::vector<cv::Point3d> execute(std::vector<cv::Point2d> pts_2d);
 
 
- protected:
+ public:
     cv::Mat K_;
     cv::Mat R_;
     cv::Mat t_;

Steger.cpp

@@ -219,8 +219,10 @@ std::vector<cv::Point2d> CSteger::StripCenter(cv::Mat image)
 	delete[] tData;
 
     auto t2=std::chrono::steady_clock::now();
-    double ms=std::chrono::duration<double,std::milli>(t2-t1).count();
-    printf("time :%lf\n",ms);
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
+    double tm=double(duration.count()) * std::chrono::milliseconds::period::num /
+            std::chrono::milliseconds::period::den;
+    printf("triangle time :%lfs\n",tm);
 
 	return g_contour;
 }

main.cpp

@@ -3,33 +3,53 @@
 #include <string>
 
 #include "Steger.h"
+#include "pnp.h"
+#include "Steger.h"
+#include <chrono>
 
+void test_pnp(){
+    cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
+    cv::Mat R=(cv::Mat_<double> ( 3,3 ) <<0.9979193252225095, -0.05138618904650328, 0.03894200717385427,
+            0.05033852907733768, 0.9983556574295407, 0.02742286944795593,
+            -0.04028712992732941, -0.02540552801471822, 0.998865109165653);
 
-void test_old()
-{
-	cv::Mat src = cv::imread("../images/3.bmp", 0);
-	cv::Mat image = src(cv::Rect(0, 0, src.cols, src.rows)).clone();
-	for (int i = 0; i < image.rows; i++)
-	{
-		for (int j = 0; j < image.cols; j++)
-		{
-			if (image.at<uchar>(i, j) < 20)
-				image.at<uchar>(i, j) = 0;
-		}
-	}
-	cv::Mat out= cv::Mat::zeros(image.size(), CV_8UC1);
-	CSteger steger;
+    cv::Mat T=(cv::Mat_<double>(3,1)<<-0.1255867099750184,-0.007363525258815341,0.0609992658867812);
+
+    P3d2d p3d2d(K);
+    p3d2d.R_=R;
+    p3d2d.t_=T;
+
+
+    cv::Mat src = cv::imread("../images/1.bmp", 0);
+    cv::Mat image = src(cv::Rect(0, 0, src.cols, src.rows)).clone();
+    for (int i = 0; i < image.rows; i++)
+    {
+        for (int j = 0; j < image.cols; j++)
+        {
+            if (image.at<uchar>(i, j) < 20)
+                image.at<uchar>(i, j) = 0;
+        }
+    }
+    cv::Mat out= cv::Mat::zeros(image.size(), CV_8UC1);
+    CSteger steger;
     cv::GaussianBlur(image, image, cv::Size(3, 3), 3, 3);
-	steger.test(image, out);
-	cv::imshow("res",out);
-	cv::waitKey(0);
-}
+    std::vector<cv::Point2d> points = steger.StripCenter(image);
+
+    auto t1=std::chrono::steady_clock::now();
+    p3d2d.execute(points);
 
+    auto t2=std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
+    double tm=double(duration.count()) * std::chrono::milliseconds::period::num /
+            std::chrono::milliseconds::period::den;
+    printf("points :%d  time :%lf s\n",points.size(),tm);
 
+
+}
 int main()
 {
 
-    test_old();
+    test_pnp();
     /*Mat image = cv::imread(file, 0);
     	Mat image = src(Rect(0, 800, src.cols, 500));
         for (int i = 0; i < image.rows; i++)

PnP/pose_estimation_3d2d.cpp

@@ -13,6 +13,8 @@
 #include <g2o/types/sba/types_six_dof_expmap.h>*/
 #include <chrono>
 
+#include "pnp.h"
+#include "../Steger.h"
 using namespace std;
 using namespace cv;
 
@@ -22,10 +24,7 @@ void find_feature_matches (
     std::vector<KeyPoint>& keypoints_2,
     std::vector< DMatch >& matches );
 
-// 像素坐标转相机归一化坐标
-Point2d pixel2cam ( const Point2d& p, const Mat& K );
 
-Point3d pixel2world ( const Point2f& p, const Mat& K );
 
 void bundleAdjustment (
     const vector<Point3f> points_3d,
@@ -72,8 +71,51 @@ void verify_result(Mat R,Mat t,vector<Point2f> pts_2d,Mat K,vector<Point3f> pts_
 
 }
 
+
+
+void test_pnp(){
+    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
+    cv::Mat R=(Mat_<double> ( 3,3 ) <<0.9979193252225095, -0.05138618904650328, 0.03894200717385427,
+            0.05033852907733768, 0.9983556574295407, 0.02742286944795593,
+            -0.04028712992732941, -0.02540552801471822, 0.998865109165653);
+
+    cv::Mat T=(Mat_<double>(3,1)<<-0.1255867099750184,-0.007363525258815341,0.0609992658867812);
+
+    P3d2d p3d2d(K);
+    p3d2d.R_=R;
+    p3d2d.t_=T;
+
+
+    cv::Mat src = cv::imread("../images/3.bmp", 0);
+    cv::Mat image = src(cv::Rect(0, 0, src.cols, src.rows)).clone();
+    for (int i = 0; i < image.rows; i++)
+    {
+        for (int j = 0; j < image.cols; j++)
+        {
+            if (image.at<uchar>(i, j) < 20)
+                image.at<uchar>(i, j) = 0;
+        }
+    }
+    cv::Mat out= cv::Mat::zeros(image.size(), CV_8UC1);
+    CSteger steger;
+    cv::GaussianBlur(image, image, cv::Size(3, 3), 3, 3);
+    std::vector<cv::Point2d> points = steger.StripCenter(image);
+
+    auto t1=std::chrono::steady_clock::now();
+    p3d2d.execute(points);
+
+    auto t2=std::chrono::steady_clock::now();
+    double ms=std::chrono::duration<double,std::milli>(t2-t1).count();
+    printf("points :%d  time :%lf\n",points.size(),ms);
+
+
+}
+
 int main ( int argc, char** argv )
 {
+    test_pnp();
+    return 0;
+
     if ( argc != 4 )
     {
         cout<<"usage: pose_estimation_3d2d img1 img2 depth1"<<endl;
@@ -174,14 +216,6 @@ void find_feature_matches ( const Mat& img_1, const Mat& img_2,
     }
 }
 
-Point2d pixel2cam ( const Point2d& p, const Mat& K )
-{
-    return Point2d
-           (
-               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
-               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
-           );
-}
 
 void bundleAdjustment (
     const vector< Point3f > points_3d,