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

added distortion coefs support into gpu::solvePnpRansac

Browse Source
This commit is contained in:
Alexey Spizhevoy 2011-03-01 07:44:36 +00:00
parent 513997e127
commit 5e85566477
2 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
modules/gpu/src/calib3d.cpp
View File

@ -148,7 +148,7 @@ namespace cv { namespace gpu { namespace solve_pnp_ransac
void computeHypothesisScores( void computeHypothesisScores(
const int num_hypotheses, const int num_points, const float* rot_matrices, const int num_hypotheses, const int num_points, const float* rot_matrices,
const float3* transl_vectors, const float3* object, const float2* image, const float3* transl_vectors, const float3* object, const float2* image,
const float3* camera_mat, const float dist_threshold, int* hypothesis_scores); const float dist_threshold, int* hypothesis_scores);
}}} }}}
namespace namespace
@ -178,16 +178,15 @@ namespace
class TransformHypothesesGenerator class TransformHypothesesGenerator
{ {
public: public:
TransformHypothesesGenerator(const Mat& object_, const Mat& image_, const Mat& camera_mat_, TransformHypothesesGenerator(const Mat& object_, const Mat& image_, const Mat& dist_coef_,
int num_points_, int subset_size_, Mat rot_matrices_, Mat transl_vectors_) const Mat& camera_mat_, int num_points_, int subset_size_,
: object(&object_), image(&image_), camera_mat(&camera_mat_), num_points(num_points_), Mat rot_matrices_, Mat transl_vectors_)
subset_size(subset_size_), rot_matrices(rot_matrices_), transl_vectors(transl_vectors_) {} : object(&object_), image(&image_), dist_coef(&dist_coef_), camera_mat(&camera_mat_),
num_points(num_points_), subset_size(subset_size_), rot_matrices(rot_matrices_),
transl_vectors(transl_vectors_) {}
void operator()(const BlockedRange& range) const void operator()(const BlockedRange& range) const
{ {
// We assume that input is undistorted
Mat empty_dist_coef;
// Input data for generation of the current hypothesis // Input data for generation of the current hypothesis
vector<int> subset_indices(subset_size); vector<int> subset_indices(subset_size);
Mat_<Point3f> object_subset(1, subset_size); Mat_<Point3f> object_subset(1, subset_size);
@ -207,7 +206,7 @@ namespace
image_subset(0, i) = image->at<Point2f>(subset_indices[i]); image_subset(0, i) = image->at<Point2f>(subset_indices[i]);
} }
solvePnP(object_subset, image_subset, *camera_mat, empty_dist_coef, rot_vec, transl_vec); solvePnP(object_subset, image_subset, *camera_mat, *dist_coef, rot_vec, transl_vec);
// Remember translation vector // Remember translation vector
Mat transl_vec_ = transl_vectors.colRange(iter * 3, (iter + 1) * 3); Mat transl_vec_ = transl_vectors.colRange(iter * 3, (iter + 1) * 3);
@ -223,6 +222,7 @@ namespace
const Mat* object; const Mat* object;
const Mat* image; const Mat* image;
const Mat* dist_coef;
const Mat* camera_mat; const Mat* camera_mat;
int num_points; int num_points;
int subset_size; int subset_size;
@ -240,28 +240,33 @@ void cv::gpu::solvePnpRansac(const Mat& object, const Mat& image, const Mat& cam
CV_Assert(image.rows == 1 && image.cols > 0 && image.type() == CV_32FC2); CV_Assert(image.rows == 1 && image.cols > 0 && image.type() == CV_32FC2);
CV_Assert(object.cols == image.cols); CV_Assert(object.cols == image.cols);
CV_Assert(camera_mat.size() == Size(3, 3) && camera_mat.type() == CV_32F); CV_Assert(camera_mat.size() == Size(3, 3) && camera_mat.type() == CV_32F);
CV_Assert(dist_coef.empty()); // We don't support undistortion for now
CV_Assert(!params.use_extrinsic_guess); // We don't support initial guess for now CV_Assert(!params.use_extrinsic_guess); // We don't support initial guess for now
const int num_points = object.cols; const int num_points = object.cols;
// Unapply distortion and intrinsic camera transformations
Mat eye_camera_mat = Mat::eye(3, 3, CV_32F);
Mat empty_dist_coef;
Mat image_normalized;
undistortPoints(image, image_normalized, camera_mat, dist_coef, Mat(), eye_camera_mat);
// Hypotheses storage (global) // Hypotheses storage (global)
Mat rot_matrices(1, params.num_iters * 9, CV_32F); Mat rot_matrices(1, params.num_iters * 9, CV_32F);
Mat transl_vectors(1, params.num_iters * 3, CV_32F); Mat transl_vectors(1, params.num_iters * 3, CV_32F);
// Generate set of hypotheses using small subsets of the input data // Generate set of hypotheses using small subsets of the input data
TransformHypothesesGenerator body(object, image, camera_mat, num_points, TransformHypothesesGenerator body(object, image_normalized, empty_dist_coef, eye_camera_mat,
params.subset_size, rot_matrices, transl_vectors); num_points, params.subset_size, rot_matrices, transl_vectors);
parallel_for(BlockedRange(0, params.num_iters), body); parallel_for(BlockedRange(0, params.num_iters), body);
// Compute scores (i.e. number of inliers) for each hypothesis // Compute scores (i.e. number of inliers) for each hypothesis
GpuMat d_object(object); GpuMat d_object(object);
GpuMat d_image(image); GpuMat d_image_normalized(image_normalized);
GpuMat d_hypothesis_scores(1, params.num_iters, CV_32S); GpuMat d_hypothesis_scores(1, params.num_iters, CV_32S);
solve_pnp_ransac::computeHypothesisScores( solve_pnp_ransac::computeHypothesisScores(
params.num_iters, num_points, rot_matrices.ptr<float>(), transl_vectors.ptr<float3>(), params.num_iters, num_points, rot_matrices.ptr<float>(), transl_vectors.ptr<float3>(),
d_object.ptr<float3>(), d_image.ptr<float2>(), camera_mat.ptr<float3>(), d_object.ptr<float3>(), d_image_normalized.ptr<float2>(), params.max_dist * params.max_dist,
params.max_dist * params.max_dist, d_hypothesis_scores.ptr<int>()); d_hypothesis_scores.ptr<int>());
// Find the best hypothesis index // Find the best hypothesis index
Point best_idx; Point best_idx;
@ -296,9 +301,9 @@ void cv::gpu::solvePnpRansac(const Mat& object, const Mat& image, const Mat& cam
p_transf.z = rot[6] * p.x + rot[7] * p.y + rot[8] * p.z + transl[2]; p_transf.z = rot[6] * p.x + rot[7] * p.y + rot[8] * p.z + transl[2];
if (p_transf.z > 0.f) if (p_transf.z > 0.f)
{ {
p_proj.x = camera_mat.at<float>(0, 0) * p_transf.x / p_transf.z + camera_mat.at<float>(0, 2); p_proj.x = p_transf.x / p_transf.z;
p_proj.y = camera_mat.at<float>(1, 1) * p_transf.x / p_transf.z + camera_mat.at<float>(1, 2); p_proj.y = p_transf.y / p_transf.z;
if (norm(p_proj - image.at<Point2f>(0, i)) < params.max_dist) if (norm(p_proj - image_normalized.at<Point2f>(0, i)) < params.max_dist)
(*params.inliers)[inlier_id++] = i; (*params.inliers)[inlier_id++] = i;
} }
} }

8
modules/gpu/src/cuda/calib3d.cu
View File

@ -122,7 +122,6 @@ namespace cv { namespace gpu
{ {
__constant__ float3 crot_matrices[SOLVE_PNP_RANSAC_NUM_ITERS * 3]; __constant__ float3 crot_matrices[SOLVE_PNP_RANSAC_NUM_ITERS * 3];
__constant__ float3 ctransl_vectors[SOLVE_PNP_RANSAC_NUM_ITERS]; __constant__ float3 ctransl_vectors[SOLVE_PNP_RANSAC_NUM_ITERS];
__constant__ float3 ccamera_mat[2];
__device__ float sqr(float x) __device__ float sqr(float x)
{ {
@ -146,8 +145,8 @@ namespace cv { namespace gpu
rot_mat[2].x * p.x + rot_mat[2].y * p.y + rot_mat[2].z * p.z + transl_vec.z); rot_mat[2].x * p.x + rot_mat[2].y * p.y + rot_mat[2].z * p.z + transl_vec.z);
if (p.z > 0) if (p.z > 0)
{ {
p.x = ccamera_mat[0].x * p.x / p.z + ccamera_mat[0].z; p.x /= p.z;
p.y = ccamera_mat[1].y * p.y / p.z + ccamera_mat[1].z; p.y /= p.z;
float2 image_p = image[i]; float2 image_p = image[i];
if (sqr(p.x - image_p.x) + sqr(p.y - image_p.y) < dist_threshold) if (sqr(p.x - image_p.x) + sqr(p.y - image_p.y) < dist_threshold)
++num_inliers; ++num_inliers;
@ -172,11 +171,10 @@ namespace cv { namespace gpu
void computeHypothesisScores( void computeHypothesisScores(
const int num_hypotheses, const int num_points, const float* rot_matrices, const int num_hypotheses, const int num_points, const float* rot_matrices,
const float3* transl_vectors, const float3* object, const float2* image, const float3* transl_vectors, const float3* object, const float2* image,
const float3* camera_mat, const float dist_threshold, int* hypothesis_scores) const float dist_threshold, int* hypothesis_scores)
{ {
cudaSafeCall(cudaMemcpyToSymbol(crot_matrices, rot_matrices, num_hypotheses * 3 * sizeof(float3))); cudaSafeCall(cudaMemcpyToSymbol(crot_matrices, rot_matrices, num_hypotheses * 3 * sizeof(float3)));
cudaSafeCall(cudaMemcpyToSymbol(ctransl_vectors, transl_vectors, num_hypotheses * sizeof(float3))); cudaSafeCall(cudaMemcpyToSymbol(ctransl_vectors, transl_vectors, num_hypotheses * sizeof(float3)));
cudaSafeCall(cudaMemcpyToSymbol(ccamera_mat, camera_mat, 2 * sizeof(float3)));
dim3 threads(256); dim3 threads(256);
dim3 grid(num_hypotheses); dim3 grid(num_hypotheses);