initial commit

donglinb · Sep 28, 2020 · 6196cf6 · 6196cf6
1 parent af9c63a
commit 6196cf6
Show file tree

Hide file tree

Showing 37 changed files with 4,294 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8)
+project(pnpl)
+
+if(NOT CMAKE_BUILD_TYPE)
+  SET(CMAKE_BUILD_TYPE Release)
+endif()
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
+
+find_package(Eigen3 REQUIRED)
+message("-- Found Eigen3 ${Eigen3_VERSION} at ${EIGEN3_INCLUDE_DIR}")
+
+include_directories(
+${PROJECT_SOURCE_DIR}
+${EIGEN3_INCLUDE_DIR}
+)
+
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+
+aux_source_directory(./PoseLib/ POSELIB_SRCS)
+add_library(pose_lib SHARED ${POSELIB_SRCS})
+
+aux_source_directory(./PoseComputer/ POSE_COMPUTER_SRCS)
+add_library(pose_computer SHARED ${POSE_COMPUTER_SRCS})
+target_link_libraries(pose_computer pose_lib)
+
+aux_source_directory(./RANSAC/ RANSAC_SRCS)
+add_library(ransac SHARED ${RANSAC_SRCS})
+target_link_libraries(ransac pose_computer)
+
+aux_source_directory(./PnPL/ PNPL_SRCS)
+add_library(pnpl SHARED ${PNPL_SRCS})
+target_link_libraries(pnpl pose_computer ransac)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build)
+
+add_executable(benchmark test/benchmark.cc test/problem_generator.cc)
+target_link_libraries(benchmark pose_lib)
+
+add_executable(test_pnpl test/test_pnpl.cpp test/problem_generator.cc)
+target_link_libraries(test_pnpl pnpl)
+
+add_executable(test_pnpl2 test/test_pnpl2.cpp)
+target_link_libraries(test_pnpl2 pnpl)
+
diff --git a/PnPL/pnpl.cpp b/PnPL/pnpl.cpp
@@ -0,0 +1,141 @@
+#include "pnpl.h"
+#include "PoseComputer/AdapterPointLine.h"
+#include "RANSAC/AbsolutePoseSacProblem.hpp"
+#include "RANSAC/Ransac.hpp"
+
+#include <memory>
+
+namespace pnpl
+{
+
+using pose_lib::AdapterPointLine;
+using opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem;
+using opengv::sac::Ransac;
+
+void PnPL(const std::vector<Eigen::Vector3d>& points_2d, const std::vector<Eigen::Vector3d>& points_3d, 
+          const std::vector<Eigen::Vector3d>& lines_2d_coeff, const std::vector<Eigen::Vector3d>& lines_3d_midpoint, 
+          const std::vector<Eigen::Vector3d>& lines_3d_direction, CameraPose& pose, 
+          std::vector<double>* weights, double threshold, double focal)
+{
+    bool useWeights = false;
+    if(weights)
+        useWeights = true;
+
+    // create adapter
+    AdapterPointLine adapter(useWeights);
+    int idx_adapter = 0;
+    if(useWeights)
+    {
+        for(int i=0;i<points_2d.size();i++)
+        {
+            adapter.addPointPair(points_2d[i], points_3d[i], (*weights)[idx_adapter++]);
+        }
+        for(int i=0;i<lines_2d_coeff.size();i++)
+        {
+            adapter.addLinePair(lines_2d_coeff[i], lines_3d_midpoint[i], lines_3d_direction[i], (*weights)[idx_adapter++]);
+        }
+    }
+    else
+    {
+        for(int i=0;i<points_2d.size();i++)
+        {
+            adapter.addPointPair(points_2d[i], points_3d[i]);
+        }
+        for(int i=0;i<lines_2d_coeff.size();i++)
+        {
+            adapter.addLinePair(lines_2d_coeff[i], lines_3d_midpoint[i], lines_3d_direction[i]);
+        }
+    }
+
+    // create sac_problem
+    std::shared_ptr<AbsolutePoseSacProblem> sac_problem(new AbsolutePoseSacProblem(adapter, useWeights));
+
+    // create ransac
+    Ransac<AbsolutePoseSacProblem> ransac;
+    ransac.sac_model_ = sac_problem;
+    ransac.threshold_ = AdapterPointLine::PointThresholdFromPixel(threshold, focal);
+    ransac.max_iterations_ = 50;
+
+    // compute model
+    ransac.computeModel(2);
+
+    pose = ransac.model_coefficients_;
+}
+
+static Eigen::Vector3d UnProject2NormalPlane(const Eigen::Vector2d& x_p, const Eigen::Matrix3d& K)
+{
+    Eigen::Vector3d x;
+    x[0] = (x_p[0] - K(0,2)) / K(0,0);
+    x[1] = (x_p[1] - K(1,2)) / K(1,1);
+    x[2] = 1;
+
+    return x;
+}
+static Eigen::Vector3d UnProject2UnitBearing(const Eigen::Vector2d& x_p, const Eigen::Matrix3d& K)
+{
+    Eigen::Vector3d x = UnProject2NormalPlane(x_p,K);
+    return x/x.norm();
+}
+
+void PnPL(const std::vector<Eigen::Vector3d>& pts3d, const std::vector<Eigen::Vector2d>& pts2d, 
+          const std::vector<Eigen::Vector6d>& lns3d, const std::vector<Eigen::Vector4d>& lns2d, 
+          const Eigen::Matrix3d& K, Eigen::Matrix3d& R, Eigen::Vector3d& t, 
+          std::vector<double>* weights, double threshold)
+{
+    bool useWeights = false;
+    if(weights)
+        useWeights = true;
+
+    // create adapter
+    AdapterPointLine adapter(useWeights);
+    int idx_adapter = 0;
+    if(useWeights)
+    {
+        for(int i=0;i<pts3d.size();i++)
+        {
+            adapter.addPointPair(UnProject2UnitBearing(pts2d[i], K), pts3d[i], (*weights)[idx_adapter++]);
+        }
+        for(int i=0;i<lns3d.size();i++)
+        {
+            Eigen::Vector3d spl = UnProject2NormalPlane(lns2d[i].head(2), K);
+            Eigen::Vector3d epl = UnProject2NormalPlane(lns2d[i].tail(2), K);
+            Eigen::Vector3d le = spl.cross(epl);
+            Eigen::Vector3d X = 0.5*(lns3d[i].head(3) + lns3d[i].tail(3));
+            Eigen::Vector3d V = lns3d[i].tail(3) - lns3d[i].head(3);
+            adapter.addLinePair(le, X, V, (*weights)[idx_adapter++]);
+        }
+    }
+    else
+    {
+        for(int i=0;i<pts3d.size();i++)
+        {
+            adapter.addPointPair(UnProject2UnitBearing(pts2d[i], K), pts3d[i]);
+        }
+        for(int i=0;i<lns3d.size();i++)
+        {
+            Eigen::Vector3d spl = UnProject2NormalPlane(lns2d[i].head(2), K);
+            Eigen::Vector3d epl = UnProject2NormalPlane(lns2d[i].tail(2), K);
+            Eigen::Vector3d le = spl.cross(epl);
+            Eigen::Vector3d X = 0.5*(lns3d[i].head(3) + lns3d[i].tail(3));
+            Eigen::Vector3d V = lns3d[i].tail(3) - lns3d[i].head(3);
+            adapter.addLinePair(le, X, V);
+        }
+    }
+
+    // create sac_problem
+    std::shared_ptr<AbsolutePoseSacProblem> sac_problem(new AbsolutePoseSacProblem(adapter, useWeights));
+
+    // create ransac
+    Ransac<AbsolutePoseSacProblem> ransac;
+    ransac.sac_model_ = sac_problem;
+    ransac.threshold_ = AdapterPointLine::PointThresholdFromPixel(threshold, K(0,0));
+    ransac.max_iterations_ = 50;
+
+    // compute model
+    ransac.computeModel(2);
+
+    R = ransac.model_coefficients_.R;
+    t = ransac.model_coefficients_.t;
+}
+
+}  // namespace pnpl
diff --git a/PnPL/pnpl.h b/PnPL/pnpl.h
@@ -0,0 +1,31 @@
+#pragma once
+#ifndef PNPL_H_
+#define PNPL_H_
+
+#include <eigen3/Eigen/Core>
+#include <vector>
+#include "PoseLib/types.h"
+
+namespace Eigen
+{
+    typedef Matrix<double,6,1> Vector6d;
+}
+
+namespace pnpl
+{
+
+using pose_lib::CameraPose;
+
+void PnPL(const std::vector<Eigen::Vector3d>& points_2d, const std::vector<Eigen::Vector3d>& points_3d, 
+          const std::vector<Eigen::Vector3d>& lines_2d_coeff, const std::vector<Eigen::Vector3d>& lines_3d_midpoint, 
+          const std::vector<Eigen::Vector3d>& lines_3d_direction, CameraPose& pose, 
+          std::vector<double>* weights = nullptr, double threshold = 1.0, double focal = 1.0);
+
+void PnPL(const std::vector<Eigen::Vector3d>& pts3d, const std::vector<Eigen::Vector2d>& pts2d, 
+          const std::vector<Eigen::Vector6d>& lns3d, const std::vector<Eigen::Vector4d>& lns2d, 
+          const Eigen::Matrix3d& K, Eigen::Matrix3d& R, Eigen::Vector3d& t, 
+          std::vector<double>* weights = nullptr, double threshold = 1.0);
+
+}  // namespace pnpl
+
+#endif  // PNPL_H_
diff --git a/PoseComputer/AdapterBase.h b/PoseComputer/AdapterBase.h
@@ -0,0 +1,22 @@
+#pragma once
+#ifndef ADAPTER_BASE_H_
+#define ADAPTER_BASE_H_
+
+namespace pose_lib
+{
+
+class AdapterBase
+{
+public:
+    virtual int getNumberCorrespondences() = 0;
+    typedef enum CorrespondanceType
+    {
+        POINT = 0,
+        LINE
+    } correspondance_t;
+
+};  // AdapterBase
+
+}  // namespace pose_lib
+
+#endif  // ADAPTER_BASE_H_
diff --git a/PoseComputer/AdapterPointLine.cpp b/PoseComputer/AdapterPointLine.cpp
@@ -0,0 +1,139 @@
+#include "AdapterPointLine.h"
+
+namespace pose_lib
+{
+
+AdapterPointLine::AdapterPointLine(bool useWeights): useWeights(useWeights)
+{
+    Reset();
+}
+
+void AdapterPointLine::Reset()
+{
+    // point line pairs and weights
+    points_2d.clear();
+    points_3d.clear();
+    lines_2d_coeff.clear();
+    lines_3d_midpoint.clear();
+    lines_3d_direction.clear();
+    weights.clear();
+    // indices
+    PairTypes.clear();
+    indices.clear();
+    n_points = 0;
+    n_lines = 0;
+}
+
+int AdapterPointLine::getNumberCorrespondences()
+{
+    return n_points + n_lines;
+}
+
+void AdapterPointLine::addPointPair(const Eigen::Vector3d& pt2d, const Eigen::Vector3d& pt3d, double w)
+{
+    points_2d.push_back(pt2d);
+    points_3d.push_back(pt3d);
+
+    if(useWeights)
+    {
+        weights.push_back(w);
+    }
+
+    PairTypes.push_back(POINT);
+    indices.push_back(n_points);
+    n_points++;
+}
+
+void AdapterPointLine::addLinePair(const Eigen::Vector3d& l, const Eigen::Vector3d& X, const Eigen::Vector3d& V, double w)
+{
+    lines_2d_coeff.push_back(l);
+    lines_3d_midpoint.push_back(X);
+    lines_3d_direction.push_back(V);
+
+    if(useWeights)
+    {
+        weights.push_back(w);
+    }
+
+    PairTypes.push_back(LINE);
+    indices.push_back(n_lines);
+    n_lines++;
+}
+
+const Eigen::Vector3d& AdapterPointLine::getPoint2D(const int idx)
+{
+    assert(PairTypes[idx] == POINT);
+    int real_idx = indices[idx];
+    return points_2d[real_idx];
+}
+
+const Eigen::Vector3d& AdapterPointLine::getPoint3D(const int idx)
+{
+    assert(PairTypes[idx] == POINT);
+    int real_idx = indices[idx];
+    return points_3d[real_idx];
+}
+
+const Eigen::Vector3d& AdapterPointLine::getLine2D(const int idx)
+{
+    assert(PairTypes[idx] == LINE);
+    int real_idx = indices[idx];
+    return lines_2d_coeff[real_idx];
+}
+const Eigen::Vector3d& AdapterPointLine::getLine3DMidPoint(const int idx)
+{
+    assert(PairTypes[idx] == LINE);
+    int real_idx = indices[idx];
+    return lines_3d_midpoint[real_idx];
+}
+
+const Eigen::Vector3d& AdapterPointLine::getLine3DDirection(const int idx)
+{
+    assert(PairTypes[idx] == LINE);
+    int real_idx = indices[idx];
+    return lines_3d_direction[real_idx];
+}
+
+double AdapterPointLine::ReprojectionErrorPoint(const Eigen::Vector3d& pt2d, const Eigen::Vector3d& pt3d, const CameraPose& pose)
+{
+    Eigen::Vector3d Pc = pose.R * pt3d + pose.t;
+    double product = Pc.dot(pt2d);
+    double norm = Pc.norm() * pt2d.norm();
+    double error = 1.0 - product / norm;
+}
+
+double AdapterPointLine::ReprojectionErrorLine(const Eigen::Vector3d& l, const Eigen::Vector3d& X, const Eigen::Vector3d& V, const CameraPose& pose)
+{
+    Eigen::Vector3d Xc = pose.R * X + pose.t;
+    Eigen::Vector3d Vc = pose.R * V;
+    double l_norm = l.norm();
+    double err1 = l.dot(Xc) / (l_norm*Xc.norm());
+    double err2 = l.dot(Vc) / (l_norm*Vc.norm());
+    double error = std::fabs(err1) + std::fabs(err2);
+
+    return error;
+}
+
+double AdapterPointLine::ReprojectionError(const int idx, const CameraPose& pose)
+{
+    int real_idx = indices[idx];
+    double error = 0.0;
+    if(PairTypes[idx] == POINT)
+    {
+        error = ReprojectionErrorPoint(points_2d[real_idx],points_3d[real_idx],pose);
+    }
+    else
+    {
+        error = ReprojectionErrorLine(lines_2d_coeff[real_idx],lines_3d_midpoint[real_idx],lines_3d_direction[real_idx],pose);
+    }
+    return error;
+}
+
+double AdapterPointLine::PointThresholdFromPixel(double th_p, double focal)
+{
+    double q = atan(th_p/focal);
+    double th = 1.0 - cos(q);
+    return th;
+}
+
+}  // namespace pose_lib