generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Removed whitespaces

Browse Source
This commit is contained in:
edgarriba 2014-08-05 17:02:06 +02:00
parent 2353436cb5
commit 0d2bc9b0a1
4 changed files with 609 additions and 828 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

965
modules/calib3d/src/dls.cpp
View File

File diff suppressed because it is too large Load Diff

88
modules/calib3d/src/dls.h
View File

@ -9,61 +9,61 @@ using namespace cv;
class dls class dls
{ {
public: public:
dls(const cv::Mat& opoints, const cv::Mat& ipoints); dls(const cv::Mat& opoints, const cv::Mat& ipoints);
~dls(); ~dls();
bool compute_pose(cv::Mat& R, cv::Mat& t); bool compute_pose(cv::Mat& R, cv::Mat& t);
private: private:
// initialisation // initialisation
template <typename OpointType, typename IpointType> template <typename OpointType, typename IpointType>
void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
{ {
for(int i = 0; i < N; i++) for(int i = 0; i < N; i++)
{ {
p.at<double>(0,i) = opoints.at<OpointType>(0,i).x; 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>(1,i) = opoints.at<OpointType>(0,i).y;
p.at<double>(2,i) = opoints.at<OpointType>(0,i).z; 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>(0,i) = ipoints.at<IpointType>(0,i).x;
z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y; z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y;
z.at<double>(2,i) = (double)1; z.at<double>(2,i) = (double)1;
} }
} }
void norm_z_vector(); void norm_z_vector();
// main algorithm // main algorithm
void run_kernel(const cv::Mat& pp); void run_kernel(const cv::Mat& pp);
void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D); 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, void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag); cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
void fill_coeff(const cv::Mat * D); void fill_coeff(const cv::Mat * D);
// useful functions // useful functions
cv::Mat LeftMultVec(const cv::Mat& v); cv::Mat LeftMultVec(const cv::Mat& v);
cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b, 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); const std::vector<double>& c, const std::vector<double>& u);
cv::Mat Hessian(const double s[]); cv::Mat Hessian(const double s[]);
cv::Mat cayley2rotbar(const cv::Mat& s); cv::Mat cayley2rotbar(const cv::Mat& s);
cv::Mat skewsymm(const cv::Mat * X1); cv::Mat skewsymm(const cv::Mat * X1);
// extra functions // extra functions
cv::Mat rotx(const double t); cv::Mat rotx(const double t);
cv::Mat roty(const double t); cv::Mat roty(const double t);
cv::Mat rotz(const double t); cv::Mat rotz(const double t);
cv::Mat mean(const cv::Mat& M); cv::Mat mean(const cv::Mat& M);
bool is_empty(const cv::Mat * v); bool is_empty(const cv::Mat * v);
bool positive_eigenvalues(const cv::Mat * eigenvalues); bool positive_eigenvalues(const cv::Mat * eigenvalues);
cv::Mat p, z; // object-image points cv::Mat p, z; // object-image points
int N; // number of input points int N; // number of input points
std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
std::vector<cv::Mat> C_est_, t_est_; // optimal candidates std::vector<cv::Mat> C_est_, t_est_; // optimal candidates
cv::Mat C_est__, t_est__; // optimal found solution cv::Mat C_est__, t_est__; // optimal found solution
double cost__; // optimal found solution double cost__; // optimal found solution
}; };

310
modules/calib3d/src/solvepnp.cpp
View File

@ -96,15 +96,15 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
} }
else if (flags == DLS) else if (flags == DLS)
{ {
cv::Mat undistortedPoints; cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
dls PnP(opoints, undistortedPoints); dls PnP(opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = PnP.compute_pose(R, tvec); bool result = PnP.compute_pose(R, tvec);
if (result) if (result)
cv::Rodrigues(R, rvec); cv::Rodrigues(R, rvec);
return result; return result;
} }
else else
@ -112,189 +112,14 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
return false; 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 class PnPRansacCallback : public PointSetRegistrator::Callback
{ {
public: public:
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE, 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() ) bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
rvec(_rvec), tvec(_tvec) {} rvec(_rvec), tvec(_tvec) {}
/* Pre: True */ /* Pre: True */
/* Post: compute _model with given points an return number of found models */ /* Post: compute _model with given points an return number of found models */
@ -303,11 +128,11 @@ public:
Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
rvec, tvec, useExtrinsicGuess, flags ); rvec, tvec, useExtrinsicGuess, flags );
Mat _local_model; Mat _local_model;
cv::hconcat(rvec, tvec, _local_model); cv::hconcat(rvec, tvec, _local_model);
_local_model.copyTo(_model); _local_model.copyTo(_model);
return correspondence; return correspondence;
} }
@ -333,7 +158,7 @@ public:
float* err = _err.getMat().ptr<float>(); float* err = _err.getMat().ptr<float>();
for ( i = 0; i < count; ++i) 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 Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec); cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec);
int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points int model_points = flags == cv::P3P ? 4 : 6; // minimum of number of model points
double param1 = reprojectionError; // reprojection error double param1 = reprojectionError; // reprojection error
double param2 = confidence; // confidence double param2 = confidence; // confidence
int param3 = iterationsCount; // number maximum iterations int param3 = iterationsCount; // number maximum iterations
cv::Mat _local_model(3, 2, CV_64FC1); cv::Mat _local_model(3, 2, CV_64FC1);
cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
// call Ransac // call Ransac
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
if( result <= 0 || _local_model.rows <= 0) 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)
{ {
parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params, _rvec.assign(rvec); // output rotation vector
localRvec, localTvec, localInliers)); _tvec.assign(tvec); // output translation vector
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT) if( _inliers.needed() )
{ _inliers.release();
if (flags != P3P)
{ return false;
int i, pointsCount = (int)localInliers.size();
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
for (i = 0; i < pointsCount; i++)
{
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);
}
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);
} }
else else
{ {
tvec.setTo(Scalar(0)); _rvec.assign(_local_model.col(0)); // output rotation vector
Mat R = Mat::eye(3, 3, CV_64F); _tvec.assign(_local_model.col(1)); // output translation vector
Rodrigues(R, rvec); }
if( _inliers.needed() )
_inliers.release(); 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);
}
return true; return true;
} }

74
samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/CMakeLists.txt
View File

@ -2,58 +2,58 @@ cmake_minimum_required(VERSION 2.8)
project( PNP_DEMO ) project( PNP_DEMO )
ADD_DEFINITIONS( ADD_DEFINITIONS(
-std=c++11 -std=c++11
# Other flags # Other flags
) )
find_package( OpenCV REQUIRED ) find_package( OpenCV REQUIRED )
include_directories( include_directories(
${OpenCV_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
) )
add_executable( add_executable(
pnp_registration pnp_registration
src/main_registration.cpp src/main_registration.cpp
src/CsvReader.cpp src/CsvReader.cpp
src/CsvWriter.cpp src/CsvWriter.cpp
src/ModelRegistration.cpp src/ModelRegistration.cpp
src/Mesh.cpp src/Mesh.cpp
src/Model.cpp src/Model.cpp
src/PnPProblem.cpp src/PnPProblem.cpp
src/Utils.cpp src/Utils.cpp
src/RobustMatcher.cpp src/RobustMatcher.cpp
) )
add_executable( add_executable(
pnp_verification pnp_verification
src/main_verification.cpp src/main_verification.cpp
src/CsvReader.cpp src/CsvReader.cpp
src/CsvWriter.cpp src/CsvWriter.cpp
src/ModelRegistration.cpp src/ModelRegistration.cpp
src/Mesh.cpp src/Mesh.cpp
src/Model.cpp src/Model.cpp
src/PnPProblem.cpp src/PnPProblem.cpp
src/Utils.cpp src/Utils.cpp
src/RobustMatcher.cpp src/RobustMatcher.cpp
) )
add_executable( add_executable(
pnp_detection pnp_detection
src/main_detection.cpp src/main_detection.cpp
src/CsvReader.cpp src/CsvReader.cpp
src/CsvWriter.cpp src/CsvWriter.cpp
src/ModelRegistration.cpp src/ModelRegistration.cpp
src/Mesh.cpp src/Mesh.cpp
src/Model.cpp src/Model.cpp
src/PnPProblem.cpp src/PnPProblem.cpp
src/Utils.cpp src/Utils.cpp
src/RobustMatcher.cpp src/RobustMatcher.cpp
) )
add_executable( add_executable(
pnp_test pnp_test
src/test_pnp.cpp src/test_pnp.cpp
) )
target_link_libraries( pnp_registration ${OpenCV_LIBS} ) target_link_libraries( pnp_registration ${OpenCV_LIBS} )