Skip to content

O
opencv
Commit 0d2bc9b0 authored by edgarriba's avatar edgarriba
Browse files
Options

Removed whitespaces

parent 2353436c
Hide whitespace changes
Inline Side-by-side
Showing with 131 additions and 349 deletions
modules/calib3d/src/dls.cpp
View file @ 0d2bc9b0
This source diff could not be displayed because it is too large. You can view the blob instead.
modules/calib3d/src/dls.h
View file @ 0d2bc9b0
......@@ -9,61 +9,61 @@ using namespace cv;
class dls
{
public:
dls(const cv::Mat& opoints, const cv::Mat& ipoints);
~dls();
dls(const cv::Mat& opoints, const cv::Mat& ipoints);
~dls();
bool compute_pose(cv::Mat& R, cv::Mat& t);
bool compute_pose(cv::Mat& R, cv::Mat& t);
private:
// initialisation
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
for(int i = 0; i < N; i++)
{
p.at<double>(0,i) = opoints.at<OpointType>(0,i).x;
p.at<double>(1,i) = opoints.at<OpointType>(0,i).y;
p.at<double>(2,i) = opoints.at<OpointType>(0,i).z;
z.at<double>(0,i) = ipoints.at<IpointType>(0,i).x;
z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y;
z.at<double>(2,i) = (double)1;
}
}
void norm_z_vector();
// main algorithm
void run_kernel(const cv::Mat& pp);
void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
void fill_coeff(const cv::Mat * D);
// useful functions
cv::Mat LeftMultVec(const cv::Mat& v);
cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
const std::vector<double>& c, const std::vector<double>& u);
cv::Mat Hessian(const double s[]);
cv::Mat cayley2rotbar(const cv::Mat& s);
cv::Mat skewsymm(const cv::Mat * X1);
// extra functions
cv::Mat rotx(const double t);
cv::Mat roty(const double t);
cv::Mat rotz(const double t);
cv::Mat mean(const cv::Mat& M);
bool is_empty(const cv::Mat * v);
bool positive_eigenvalues(const cv::Mat * eigenvalues);
cv::Mat p, z; // object-image points
int N; // number of input points
std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
std::vector<cv::Mat> C_est_, t_est_; // optimal candidates
cv::Mat C_est__, t_est__; // optimal found solution
double cost__; // optimal found solution
// initialisation
template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{
for(int i = 0; i < N; i++)
{
p.at<double>(0,i) = opoints.at<OpointType>(0,i).x;
p.at<double>(1,i) = opoints.at<OpointType>(0,i).y;
p.at<double>(2,i) = opoints.at<OpointType>(0,i).z;
z.at<double>(0,i) = ipoints.at<IpointType>(0,i).x;
z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y;
z.at<double>(2,i) = (double)1;
}
}
void norm_z_vector();
// main algorithm
void run_kernel(const cv::Mat& pp);
void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
void fill_coeff(const cv::Mat * D);
// useful functions
cv::Mat LeftMultVec(const cv::Mat& v);
cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
const std::vector<double>& c, const std::vector<double>& u);
cv::Mat Hessian(const double s[]);
cv::Mat cayley2rotbar(const cv::Mat& s);
cv::Mat skewsymm(const cv::Mat * X1);
// extra functions
cv::Mat rotx(const double t);
cv::Mat roty(const double t);
cv::Mat rotz(const double t);
cv::Mat mean(const cv::Mat& M);
bool is_empty(const cv::Mat * v);
bool positive_eigenvalues(const cv::Mat * eigenvalues);
cv::Mat p, z; // object-image points
int N; // number of input points
std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
std::vector<cv::Mat> C_est_, t_est_; // optimal candidates
cv::Mat C_est__, t_est__; // optimal found solution
double cost__; // optimal found solution
};
......
modules/calib3d/src/solvepnp.cpp
View file @ 0d2bc9b0
......@@ -96,15 +96,15 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
}
else if (flags == DLS)
{
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
dls PnP(opoints, undistortedPoints);
dls PnP(opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = PnP.compute_pose(R, tvec);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = PnP.compute_pose(R, tvec);
if (result)
cv::Rodrigues(R, rvec);
cv::Rodrigues(R, rvec);
return result;
}
else
......@@ -112,189 +112,14 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
return false;
}
/*namespace cv
{
namespace pnpransac
{
const int MIN_POINTS_COUNT = 4;
static void project3dPoints(const Mat& points, const Mat& rvec, const Mat& tvec, Mat& modif_points)
{
modif_points.create(1, points.cols, CV_32FC3);
Mat R(3, 3, CV_64FC1);
Rodrigues(rvec, R);
Mat transformation(3, 4, CV_64F);
Mat r = transformation.colRange(0, 3);
R.copyTo(r);
Mat t = transformation.colRange(3, 4);
tvec.copyTo(t);
transform(points, modif_points, transformation);
}
struct CameraParameters
{
void init(Mat _intrinsics, Mat _distCoeffs)
{
_intrinsics.copyTo(intrinsics);
_distCoeffs.copyTo(distortion);
}
Mat intrinsics;
Mat distortion;
};
struct Parameters
{
int iterationsCount;
float reprojectionError;
int minInliersCount;
bool useExtrinsicGuess;
int flags;
CameraParameters camera;
};
static void pnpTask(const std::vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, std::vector<int>& inliers, Mat& rvec, Mat& tvec,
const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
{
Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2);
for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
{
if (pointsMask[i])
{
Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1));
imagePoints.col(i).copyTo(colModelImagePoints);
Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1));
objectPoints.col(i).copyTo(colModelObjectPoints);
colIndex = colIndex+1;
}
}
//filter same 3d points, hang in solvePnP
double eps = 1e-10;
int num_same_points = 0;
for (int i = 0; i < MIN_POINTS_COUNT; i++)
for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
{
if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps)
num_same_points++;
}
if (num_same_points > 0)
return;
Mat localRvec, localTvec;
rvecInit.copyTo(localRvec);
tvecInit.copyTo(localTvec);
solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec,
params.useExtrinsicGuess, params.flags);
std::vector<Point2f> projected_points;
projected_points.resize(objectPoints.cols);
projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
Mat rotatedPoints;
project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
std::vector<int> localInliers;
for (int i = 0; i < objectPoints.cols; i++)
{
Point2f p(imagePoints.at<Vec2f>(0, i)[0], imagePoints.at<Vec2f>(0, i)[1]);
if ((norm(p - projected_points[i]) < params.reprojectionError)
&& (rotatedPoints.at<Vec3f>(0, i)[2] > 0)) //hack
{
localInliers.push_back(i);
}
}
if (localInliers.size() > inliers.size())
{
resultsMutex.lock();
inliers.clear();
inliers.resize(localInliers.size());
memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
resultsMutex.unlock();
}
}
class PnPSolver
{
public:
void operator()( const BlockedRange& r ) const
{
std::vector<char> pointsMask(objectPoints.cols, 0);
memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
for( int i=r.begin(); i!=r.end(); ++i )
{
generateVar(pointsMask);
pnpTask(pointsMask, objectPoints, imagePoints, parameters,
inliers, rvec, tvec, initRvec, initTvec, syncMutex);
if ((int)inliers.size() >= parameters.minInliersCount)
{
#ifdef HAVE_TBB
tbb::task::self().cancel_group_execution();
#else
break;
#endif
}
}
}
PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
Mat& _rvec, Mat& _tvec, std::vector<int>& _inliers):
objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
rvec(_rvec), tvec(_tvec), inliers(_inliers)
{
rvec.copyTo(initRvec);
tvec.copyTo(initTvec);
generator.state = theRNG().state; //to control it somehow...
}
private:
PnPSolver& operator=(const PnPSolver&);
const Mat& objectPoints;
const Mat& imagePoints;
const Parameters& parameters;
Mat &rvec, &tvec;
std::vector<int>& inliers;
Mat initRvec, initTvec;
static RNG generator;
static Mutex syncMutex;
void generateVar(std::vector<char>& mask) const
{
int size = (int)mask.size();
for (int i = 0; i < size; i++)
{
int i1 = generator.uniform(0, size);
int i2 = generator.uniform(0, size);
char curr = mask[i1];
mask[i1] = mask[i2];
mask[i2] = curr;
}
}
};
Mutex PnPSolver::syncMutex;
RNG PnPSolver::generator;
}
}*/
class PnPRansacCallback : public PointSetRegistrator::Callback
{
public:
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE,
bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
rvec(_rvec), tvec(_tvec) {}
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE,
bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
rvec(_rvec), tvec(_tvec) {}
/* Pre: True */
/* Post: compute _model with given points an return number of found models */
......@@ -303,11 +128,11 @@ public:
Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
rvec, tvec, useExtrinsicGuess, flags );
rvec, tvec, useExtrinsicGuess, flags );
Mat _local_model;
cv::hconcat(rvec, tvec, _local_model);
_local_model.copyTo(_model);
Mat _local_model;
cv::hconcat(rvec, tvec, _local_model);
_local_model.copyTo(_model);
return correspondence;
}
......@@ -333,7 +158,7 @@ public:
float* err = _err.getMat().ptr<float>();
for ( i = 0; i < count; ++i)
err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] );
err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] );
}
......@@ -375,89 +200,46 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec);
int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points
double param1 = reprojectionError; // reprojection error
double param2 = confidence; // confidence
int param3 = iterationsCount; // number maximum iterations
int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points
double param1 = reprojectionError; // reprojection error
double param2 = confidence; // confidence
int param3 = iterationsCount; // number maximum iterations
cv::Mat _local_model(3, 2, CV_64FC1);
cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
// call Ransac
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
if( result <= 0 || _local_model.rows <= 0)
{
_rvec.assign(rvec); // output rotation vector
_tvec.assign(tvec); // output translation vector
if( _inliers.needed() )
_inliers.release();
return false;
}
else
{
_rvec.assign(_local_model.col(0)); // output rotation vector
_tvec.assign(_local_model.col(1)); // output translation vector
}
if(_inliers.needed())
{
Mat _local_inliers;
int count = 0;
for (int i = 0; i < _mask_local_inliers.rows; ++i)
{
if((int)_mask_local_inliers.at<uchar>(i) == 1) // inliers mask
{
_local_inliers.push_back(count); // output inliers vector
count++;
}
}
_local_inliers.copyTo(_inliers);
}
// OLD IMPLEMENTATION
/*std::vector<int> localInliers;
Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
// call Ransac
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
if( result <= 0 || _local_model.rows <= 0)
{
parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers));
_rvec.assign(rvec); // output rotation vector
_tvec.assign(tvec); // output translation vector
if( _inliers.needed() )
_inliers.release();
return false;
}
else
{
_rvec.assign(_local_model.col(0)); // output rotation vector
_tvec.assign(_local_model.col(1)); // output translation vector
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
if(_inliers.needed())
{
if (flags != P3P)
Mat _local_inliers;
int count = 0;
for (int i = 0; i < _mask_local_inliers.rows; ++i)
{
int i, pointsCount = (int)localInliers.size();
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
for (i = 0; i < pointsCount; i++)
if((int)_mask_local_inliers.at<uchar>(i) == 1) // inliers mask
{
int index = localInliers[i];
Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
_local_inliers.push_back(count); // output inliers vector
count++;
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
Mat(localInliers).copyTo(_inliers);
_local_inliers.copyTo(_inliers);
}
else
{
tvec.setTo(Scalar(0));
Mat R = Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}*/
return true;
}
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt
View file @ 0d2bc9b0
......@@ -2,58 +2,58 @@ cmake_minimum_required(VERSION 2.8)
project( PNP_DEMO )
ADD_DEFINITIONS(
-std=c++11
-std=c++11
# Other flags
)
find_package( OpenCV REQUIRED )
include_directories(
${OpenCV_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(
pnp_registration
src/main_registration.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
add_executable(
pnp_registration
src/main_registration.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
)
add_executable(
pnp_verification
src/main_verification.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
add_executable(
pnp_verification
src/main_verification.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
)
add_executable(
add_executable(
pnp_detection
src/main_detection.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
src/main_detection.cpp
src/CsvReader.cpp
src/CsvWriter.cpp
src/ModelRegistration.cpp
src/Mesh.cpp
src/Model.cpp
src/PnPProblem.cpp
src/Utils.cpp
src/RobustMatcher.cpp
)
add_executable(
pnp_test
src/test_pnp.cpp
add_executable(
pnp_test
src/test_pnp.cpp
)
target_link_libraries( pnp_registration ${OpenCV_LIBS} )
......
Markdown is supported
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