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

gpustereo module for stereo correspondence

Browse Source
This commit is contained in:
Vladislav Vinogradov
2013-04-18 10:30:04 +04:00
parent cad9518928
commit 28b1caa730
38 changed files with 786 additions and 609 deletions
Download Patch File Download Diff File Expand all files Collapse all files

290
modules/gpu/src/calib3d.cpp Normal file
View File

@@ -0,0 +1,290 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
using namespace cv;
using namespace cv::gpu;
#if !defined HAVE_CUDA || defined(CUDA_DISABLER)
void cv::gpu::transformPoints(const GpuMat&, const Mat&, const Mat&, GpuMat&, Stream&) { throw_no_cuda(); }
void cv::gpu::projectPoints(const GpuMat&, const Mat&, const Mat&, const Mat&, const Mat&, GpuMat&, Stream&) { throw_no_cuda(); }
void cv::gpu::solvePnPRansac(const Mat&, const Mat&, const Mat&, const Mat&, Mat&, Mat&, bool, int, float, int, std::vector<int>*) { throw_no_cuda(); }
#else
namespace cv { namespace gpu { namespace cudev
{
namespace transform_points
{
void call(const PtrStepSz<float3> src, const float* rot, const float* transl, PtrStepSz<float3> dst, cudaStream_t stream);
}
namespace project_points
{
void call(const PtrStepSz<float3> src, const float* rot, const float* transl, const float* proj, PtrStepSz<float2> dst, cudaStream_t stream);
}
namespace solve_pnp_ransac
{
int maxNumIters();
void computeHypothesisScores(
const int num_hypotheses, const int num_points, const float* rot_matrices,
const float3* transl_vectors, const float3* object, const float2* image,
const float dist_threshold, int* hypothesis_scores);
}
}}}
using namespace ::cv::gpu::cudev;
namespace
{
void transformPointsCaller(const GpuMat& src, const Mat& rvec, const Mat& tvec, GpuMat& dst, cudaStream_t stream)
{
CV_Assert(src.rows == 1 && src.cols > 0 && src.type() == CV_32FC3);
CV_Assert(rvec.size() == Size(3, 1) && rvec.type() == CV_32F);
CV_Assert(tvec.size() == Size(3, 1) && tvec.type() == CV_32F);
// Convert rotation vector into matrix
Mat rot;
Rodrigues(rvec, rot);
dst.create(src.size(), src.type());
transform_points::call(src, rot.ptr<float>(), tvec.ptr<float>(), dst, stream);
}
}
void cv::gpu::transformPoints(const GpuMat& src, const Mat& rvec, const Mat& tvec, GpuMat& dst, Stream& stream)
{
transformPointsCaller(src, rvec, tvec, dst, StreamAccessor::getStream(stream));
}
namespace
{
void projectPointsCaller(const GpuMat& src, const Mat& rvec, const Mat& tvec, const Mat& camera_mat, const Mat& dist_coef, GpuMat& dst, cudaStream_t stream)
{
CV_Assert(src.rows == 1 && src.cols > 0 && src.type() == CV_32FC3);
CV_Assert(rvec.size() == Size(3, 1) && rvec.type() == CV_32F);
CV_Assert(tvec.size() == Size(3, 1) && tvec.type() == CV_32F);
CV_Assert(camera_mat.size() == Size(3, 3) && camera_mat.type() == CV_32F);
CV_Assert(dist_coef.empty()); // Undistortion isn't supported
// Convert rotation vector into matrix
Mat rot;
Rodrigues(rvec, rot);
dst.create(src.size(), CV_32FC2);
project_points::call(src, rot.ptr<float>(), tvec.ptr<float>(), camera_mat.ptr<float>(), dst,stream);
}
}
void cv::gpu::projectPoints(const GpuMat& src, const Mat& rvec, const Mat& tvec, const Mat& camera_mat, const Mat& dist_coef, GpuMat& dst, Stream& stream)
{
projectPointsCaller(src, rvec, tvec, camera_mat, dist_coef, dst, StreamAccessor::getStream(stream));
}
namespace
{
// Selects subset_size random different points from [0, num_points - 1] range
void selectRandom(int subset_size, int num_points, std::vector<int>& subset)
{
subset.resize(subset_size);
for (int i = 0; i < subset_size; ++i)
{
bool was;
do
{
subset[i] = rand() % num_points;
was = false;
for (int j = 0; j < i; ++j)
if (subset[j] == subset[i])
{
was = true;
break;
}
} while (was);
}
}
// Computes rotation, translation pair for small subsets if the input data
class TransformHypothesesGenerator : public cv::ParallelLoopBody
{
public:
TransformHypothesesGenerator(const Mat& object_, const Mat& image_, const Mat& dist_coef_,
const Mat& camera_mat_, int num_points_, int subset_size_,
Mat rot_matrices_, Mat 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 Range& range) const
{
// Input data for generation of the current hypothesis
std::vector<int> subset_indices(subset_size);
Mat_<Point3f> object_subset(1, subset_size);
Mat_<Point2f> image_subset(1, subset_size);
// Current hypothesis data
Mat rot_vec(1, 3, CV_64F);
Mat rot_mat(3, 3, CV_64F);
Mat transl_vec(1, 3, CV_64F);
for (int iter = range.start; iter < range.end; ++iter)
{
selectRandom(subset_size, num_points, subset_indices);
for (int i = 0; i < subset_size; ++i)
{
object_subset(0, i) = object->at<Point3f>(subset_indices[i]);
image_subset(0, i) = image->at<Point2f>(subset_indices[i]);
}
solvePnP(object_subset, image_subset, *camera_mat, *dist_coef, rot_vec, transl_vec);
// Remember translation vector
Mat transl_vec_ = transl_vectors.colRange(iter * 3, (iter + 1) * 3);
transl_vec = transl_vec.reshape(0, 1);
transl_vec.convertTo(transl_vec_, CV_32F);
// Remember rotation matrix
Rodrigues(rot_vec, rot_mat);
Mat rot_mat_ = rot_matrices.colRange(iter * 9, (iter + 1) * 9).reshape(0, 3);
rot_mat.convertTo(rot_mat_, CV_32F);
}
}
const Mat* object;
const Mat* image;
const Mat* dist_coef;
const Mat* camera_mat;
int num_points;
int subset_size;
// Hypotheses storage (global)
Mat rot_matrices;
Mat transl_vectors;
};
}
void cv::gpu::solvePnPRansac(const Mat& object, const Mat& image, const Mat& camera_mat,
const Mat& dist_coef, Mat& rvec, Mat& tvec, bool use_extrinsic_guess,
int num_iters, float max_dist, int min_inlier_count,
std::vector<int>* inliers)
{
(void)min_inlier_count;
CV_Assert(object.rows == 1 && object.cols > 0 && object.type() == CV_32FC3);
CV_Assert(image.rows == 1 && image.cols > 0 && image.type() == CV_32FC2);
CV_Assert(object.cols == image.cols);
CV_Assert(camera_mat.size() == Size(3, 3) && camera_mat.type() == CV_32F);
CV_Assert(!use_extrinsic_guess); // We don't support initial guess for now
CV_Assert(num_iters <= solve_pnp_ransac::maxNumIters());
const int subset_size = 4;
const int num_points = object.cols;
CV_Assert(num_points >= subset_size);
// 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)
Mat rot_matrices(1, num_iters * 9, CV_32F);
Mat transl_vectors(1, num_iters * 3, CV_32F);
// Generate set of hypotheses using small subsets of the input data
TransformHypothesesGenerator body(object, image_normalized, empty_dist_coef, eye_camera_mat,
num_points, subset_size, rot_matrices, transl_vectors);
parallel_for_(Range(0, num_iters), body);
// Compute scores (i.e. number of inliers) for each hypothesis
GpuMat d_object(object);
GpuMat d_image_normalized(image_normalized);
GpuMat d_hypothesis_scores(1, num_iters, CV_32S);
solve_pnp_ransac::computeHypothesisScores(
num_iters, num_points, rot_matrices.ptr<float>(), transl_vectors.ptr<float3>(),
d_object.ptr<float3>(), d_image_normalized.ptr<float2>(), max_dist * max_dist,
d_hypothesis_scores.ptr<int>());
// Find the best hypothesis index
Point best_idx;
double best_score;
gpu::minMaxLoc(d_hypothesis_scores, NULL, &best_score, NULL, &best_idx);
int num_inliers = static_cast<int>(best_score);
// Extract the best hypothesis data
Mat rot_mat = rot_matrices.colRange(best_idx.x * 9, (best_idx.x + 1) * 9).reshape(0, 3);
Rodrigues(rot_mat, rvec);
rvec = rvec.reshape(0, 1);
tvec = transl_vectors.colRange(best_idx.x * 3, (best_idx.x + 1) * 3).clone();
tvec = tvec.reshape(0, 1);
// Build vector of inlier indices
if (inliers != NULL)
{
inliers->clear();
inliers->reserve(num_inliers);
Point3f p, p_transf;
Point2f p_proj;
const float* rot = rot_mat.ptr<float>();
const float* transl = tvec.ptr<float>();
for (int i = 0; i < num_points; ++i)
{
p = object.at<Point3f>(0, i);
p_transf.x = rot[0] * p.x + rot[1] * p.y + rot[2] * p.z + transl[0];
p_transf.y = rot[3] * p.x + rot[4] * p.y + rot[5] * p.z + transl[1];
p_transf.z = rot[6] * p.x + rot[7] * p.y + rot[8] * p.z + transl[2];
p_proj.x = p_transf.x / p_transf.z;
p_proj.y = p_transf.y / p_transf.z;
if (norm(p_proj - image_normalized.at<Point2f>(0, i)) < max_dist)
inliers->push_back(i);
}
}
}
#endif

193
modules/gpu/src/cuda/calib3d.cu Normal file
View File

@@ -0,0 +1,193 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#if !defined CUDA_DISABLER
#include "opencv2/core/cuda/common.hpp"
#include "opencv2/core/cuda/transform.hpp"
#include "opencv2/core/cuda/functional.hpp"
#include "opencv2/core/cuda/reduce.hpp"
namespace cv { namespace gpu { namespace cudev
{
#define SOLVE_PNP_RANSAC_MAX_NUM_ITERS 200
namespace transform_points
{
__constant__ float3 crot0;
__constant__ float3 crot1;
__constant__ float3 crot2;
__constant__ float3 ctransl;
struct TransformOp : unary_function<float3, float3>
{
__device__ __forceinline__ float3 operator()(const float3& p) const
{
return make_float3(
crot0.x * p.x + crot0.y * p.y + crot0.z * p.z + ctransl.x,
crot1.x * p.x + crot1.y * p.y + crot1.z * p.z + ctransl.y,
crot2.x * p.x + crot2.y * p.y + crot2.z * p.z + ctransl.z);
}
__device__ __forceinline__ TransformOp() {}
__device__ __forceinline__ TransformOp(const TransformOp&) {}
};
void call(const PtrStepSz<float3> src, const float* rot,
const float* transl, PtrStepSz<float3> dst,
cudaStream_t stream)
{
cudaSafeCall(cudaMemcpyToSymbol(crot0, rot, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(crot1, rot + 3, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(crot2, rot + 6, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(ctransl, transl, sizeof(float) * 3));
cv::gpu::cudev::transform(src, dst, TransformOp(), WithOutMask(), stream);
}
} // namespace transform_points
namespace project_points
{
__constant__ float3 crot0;
__constant__ float3 crot1;
__constant__ float3 crot2;
__constant__ float3 ctransl;
__constant__ float3 cproj0;
__constant__ float3 cproj1;
struct ProjectOp : unary_function<float3, float3>
{
__device__ __forceinline__ float2 operator()(const float3& p) const
{
// Rotate and translate in 3D
float3 t = make_float3(
crot0.x * p.x + crot0.y * p.y + crot0.z * p.z + ctransl.x,
crot1.x * p.x + crot1.y * p.y + crot1.z * p.z + ctransl.y,
crot2.x * p.x + crot2.y * p.y + crot2.z * p.z + ctransl.z);
// Project on 2D plane
return make_float2(
(cproj0.x * t.x + cproj0.y * t.y) / t.z + cproj0.z,
(cproj1.x * t.x + cproj1.y * t.y) / t.z + cproj1.z);
}
__device__ __forceinline__ ProjectOp() {}
__device__ __forceinline__ ProjectOp(const ProjectOp&) {}
};
void call(const PtrStepSz<float3> src, const float* rot,
const float* transl, const float* proj, PtrStepSz<float2> dst,
cudaStream_t stream)
{
cudaSafeCall(cudaMemcpyToSymbol(crot0, rot, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(crot1, rot + 3, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(crot2, rot + 6, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(ctransl, transl, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(cproj0, proj, sizeof(float) * 3));
cudaSafeCall(cudaMemcpyToSymbol(cproj1, proj + 3, sizeof(float) * 3));
cv::gpu::cudev::transform(src, dst, ProjectOp(), WithOutMask(), stream);
}
} // namespace project_points
namespace solve_pnp_ransac
{
__constant__ float3 crot_matrices[SOLVE_PNP_RANSAC_MAX_NUM_ITERS * 3];
__constant__ float3 ctransl_vectors[SOLVE_PNP_RANSAC_MAX_NUM_ITERS];
int maxNumIters()
{
return SOLVE_PNP_RANSAC_MAX_NUM_ITERS;
}
__device__ __forceinline__ float sqr(float x)
{
return x * x;
}
template <int BLOCK_SIZE>
__global__ void computeHypothesisScoresKernel(
const int num_points, const float3* object, const float2* image,
const float dist_threshold, int* g_num_inliers)
{
const float3* const &rot_mat = crot_matrices + blockIdx.x * 3;
const float3 &transl_vec = ctransl_vectors[blockIdx.x];
int num_inliers = 0;
for (int i = threadIdx.x; i < num_points; i += blockDim.x)
{
float3 p = object[i];
p = make_float3(
rot_mat[0].x * p.x + rot_mat[0].y * p.y + rot_mat[0].z * p.z + transl_vec.x,
rot_mat[1].x * p.x + rot_mat[1].y * p.y + rot_mat[1].z * p.z + transl_vec.y,
rot_mat[2].x * p.x + rot_mat[2].y * p.y + rot_mat[2].z * p.z + transl_vec.z);
p.x /= p.z;
p.y /= p.z;
float2 image_p = image[i];
if (sqr(p.x - image_p.x) + sqr(p.y - image_p.y) < dist_threshold)
++num_inliers;
}
__shared__ int s_num_inliers[BLOCK_SIZE];
reduce<BLOCK_SIZE>(s_num_inliers, num_inliers, threadIdx.x, plus<int>());
if (threadIdx.x == 0)
g_num_inliers[blockIdx.x] = num_inliers;
}
void computeHypothesisScores(
const int num_hypotheses, const int num_points, const float* rot_matrices,
const float3* transl_vectors, const float3* object, const float2* image,
const float dist_threshold, int* hypothesis_scores)
{
cudaSafeCall(cudaMemcpyToSymbol(crot_matrices, rot_matrices, num_hypotheses * 3 * sizeof(float3)));
cudaSafeCall(cudaMemcpyToSymbol(ctransl_vectors, transl_vectors, num_hypotheses * sizeof(float3)));
dim3 threads(256);
dim3 grid(num_hypotheses);
computeHypothesisScoresKernel<256><<<grid, threads>>>(
num_points, object, image, dist_threshold, hypothesis_scores);
cudaSafeCall( cudaGetLastError() );
cudaSafeCall( cudaDeviceSynchronize() );
}
} // namespace solvepnp_ransac
}}} // namespace cv { namespace gpu { namespace cudev
#endif /* CUDA_DISABLER */

1
modules/gpu/src/precomp.hpp
View File

@@ -44,6 +44,7 @@
#define __OPENCV_PRECOMP_H__
#include "opencv2/gpu.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/core/gpu_private.hpp"