symao / pnpl Goto Github PK

Perspective n Point and Line for camera pose estimation.

CMake 2.04% C++ 97.96%

pnpl's Introduction

Introduction

Perspective-n-Point(PnP) is the problem of estimating the pose of a calibrated camera given a set of n 3D points in the world and their corresponding 2D projections in the image.(https://en.wikipedia.org/wiki/Perspective-n-Point)

We extend this methods to 3D-2D lines, which is called as Perspective-n-Line(PnL). Moreover, we can combine 3D-2D point pair and 3D-2D line pair together to solve the camera pose. These are implemented in pnpl.cpp(h) as a graph optimization and solved by g2o.

The pnp.cpp(h) contains close form solutions for camera position with known camera attitude(rotation). Note that we need at least 2 point pair in pnpTrans() while at least 3 line pair in pnlTrans().

Prerequisites

Prerequisites needed for compiling PnPL using c++:

OpenCV (http://opencv.org)
g2o (https://github.com/RainerKuemmerle/g2o)

Note that we oply use opencv data structures to store datas. You can change the variable type in codes if you really don't want to use opencv.

How to run

Linux

mkdir build
cd build
cmake ..
make

Windows You can use CMake-GUI to build your project.

We provide two demos to show how to use,

demo_pnpl solves camera position and rotation (6DoF) with point pairs and line pairs, it needs g2o.
demo_pnptrans solves only camera position(3DoF) with point pairs and line pairs in close form.

Licence

The source code is released under GPLv3 license.

Any problem, please contact [email protected]

pnpl's People

Contributors

Stargazers

Watchers

pnpl's Issues

Vector 4D not defined in the scope

I got several error message in pnpl.cpp. It seems that after the line
typedef Eigen::Matrix<double,6,1,Eigen::ColMajor> Vector6D;
Vector4D 3D 2D shoule also be defined.

After I fixed this, I got problems with the API of G2O. So I changed to link an early version G2O, and it is OK now.

/home/rick/SLAMProjects/PnPL/src/pnpl.cpp: In function ‘void PnPL(const std::vector<cv::Point3_ >&, const std::vector<cv::Point_ >&, const std::vector<cv::Vec<float, 6> >&, const std::vector<cv::Vec<float, 4> >&, const cv::Mat&, cv::Mat&, cv::Mat&)’:
/home/rick/SLAMProjects/PnPL/src/pnpl.cpp:143:78: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::BlockSolver(g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType*&)’
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
^
In file included from /usr/local/include/g2o/core/block_solver.h:199:0,
from /home/rick/SLAMProjects/PnPL/src/pnpl.cpp:4:
/usr/local/include/g2o/core/block_solver.hpp:40:1: note: candidate: g2o::BlockSolver::BlockSolver(std::unique_ptr) [with Traits = g2o::BlockSolverTraits<6, 3>; typename Traits::LinearSolverType = g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >]
BlockSolver::BlockSolver(std::unique_ptr linearSolver)
^
/usr/local/include/g2o/core/block_solver.hpp:40:1: note: no known conversion for argument 1 from ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType* {aka g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >}’ to ‘std::unique_ptr<g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> >, std::default_delete<g2o::LinearSolver<Eigen::Matrix<double, 6, 6, 0> > > >’
/home/rick/SLAMProjects/PnPL/src/pnpl.cpp:144:101: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(g2o::BlockSolver_6_3&)’
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
^
In file included from /home/rick/SLAMProjects/PnPL/src/pnpl.cpp:7:0:
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note: candidate: g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::unique_ptrg2o::Solver)
explicit OptimizationAlgorithmLevenberg(std::unique_ptr solver);
^
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note: no known conversion for argument 1 from ‘g2o::BlockSolver_6_3* {aka g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >*}’ to ‘std::unique_ptrg2o::Solver’
CMakeFiles/demo_pnpl.dir/build.make:86: recipe for target 'CMakeFiles/demo_pnpl.dir/src/pnpl.cpp.o' failed
make[2]: *** [CMakeFiles/demo_pnpl.dir/src/pnpl.cpp.o] Error 1
CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/demo_pnpl.dir/all' failed
make[1]: *** [CMakeFiles/demo_pnpl.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

buildSystem(): NaN within Jacobian for edge 0x56163c2901e0 for vertex 1

Hi, thank you for sharing the code!
I can run your code on my own computer. But when I modify the initial value of the translation part to 0, that is Eigen::Vector3d trans(0.0, 0.0, 0.0); in the line 148 of src/pnpnl.cpp file, I met this following error:
buildSystem(): NaN within Jacobian for edge 0x56163c2901e0 for vertex 1
And when I use Google to find this error, they said this reason is that the wrong order of the function setVertex(). But the error I met has nothing to do with the function setVertex().

Python binding

Is there any python binding for the same?

Mathematical materials

Hello! I am studying your code, is there any corresponding theoretical materials? Thank you and look forward to your reply.

Does it support RANSAC?

Hi,

Thanks for the contribution.
This is a rather general question. But does your PnPL algorithm use RANSAC to filter outliers?

Regards,
Alex

Building Error: undefined reference ...

Got a building error, the compile output shows below:

Scanning dependencies of target demo_pnptrans
[ 16%] Building CXX object CMakeFiles/demo_pnptrans.dir/src/demo_pnptrans.cpp.o
[ 33%] Building CXX object CMakeFiles/demo_pnptrans.dir/src/pnp.cpp.o
[ 50%] Linking CXX executable demo_pnptrans
/usr/bin/ld: CMakeFiles/demo_pnptrans.dir/src/demo_pnptrans.cpp.o: undefined reference to symbol '_ZTVN10__cxxabiv117__class_type_infoE@@CXXABI_1.3'
//usr/lib/x86_64-linux-gnu/libstdc++.so.6: error adding symbols: DSO missing from command line
clang-7: error: linker command failed with exit code 1 (use -v to see invocation)
make[2]: *** [demo_pnptrans] Error 1
make[1]: *** [CMakeFiles/demo_pnptrans.dir/all] Error 2
make: *** [all] Error 2

I found it only show up when I build it with 'clang'. However everything was ok when build with 'gcc'

Could you add a license?

MIT, please--if I have any say :)

the runtime error

Hi, @symao . Thanks for sharing the code.
I can run this ./demo_pnptrans succeefully. But when I run ./demo_pnpl, it have an error "Segmentation fault (core dumped)"