mirror of
https://github.com/opencv/opencv.git
synced 2024-12-05 09:49:12 +08:00
9d6f388809
New LevMarq implementation * Hash TSDF fix: apply volume pose when fetching pose * DualQuat minor fix * Pose Graph: getEdgePose(), getEdgeInfo() * debugging code for pose graph * add edge to submap * pose averaging: DualQuats instead of matrix averaging * overlapping ratio: rise it up; minor comment * remove `Submap::addEdgeToSubmap` * test_pose_graph: minor * SparseBlockMatrix: support 1xN as well as Nx1 for residual vector * small changes to old LMSolver * new LevMarq impl * Pose Graph rewritten to use new impl * solvePnP(), findHomography() and findExtrinsicCameraParams2() use new impl * estimateAffine...2D() use new impl * calibration and stereo calibration use new impl * BundleAdjusterBase::estimate() uses new impl * new LevMarq interface * PoseGraph: changing opt interface * findExtrinsicCameraParams2(): opt interface updated * HomographyRefine: opt interface updated * solvePnPRefine opt interface fixed * Affine2DRefine opt interface fixed * BundleAdjuster::estimate() opt interface fixed * calibration: opt interface fixed + code refactored a little * minor warning fixes * geodesic acceleration, Impl -> Backend rename * calcFunc() always uses probe vars * solveDecomposed, fixing negation * fixing geodesic acceleration + minors * PoseGraph exposes its optimizer now + its tests updated to check better convegence * Rosenbrock test added for LevMarq * LevMarq params upgraded * Rosenbrock can do better * fixing stereo calibration * old implementation removed (as well as debug code) * more debugging code removed * fix warnings * fixing warnings * fixing Eigen dependency * trying to fix Eigen deps * debugging code for submat is now temporary * trying to fix Eigen dependency * relax sanity check for solvePnP * relaxing sanity check even more * trying to fix Eigen dependency * warning fix * Quat<T>: fixing warnings * more warning fixes * fixed warning * fixing *KinFu OCL tests * algo params -> struct Settings * Backend moved to details * BaseLevMarq -> LevMarqBase * detail/pose_graph.hpp -> detail/optimizer.hpp * fixing include stuff for details/optimizer.hpp * doc fix * LevMarqBase rework: Settings, pImpl, Backend * Impl::settings and ::backend fix * HashTSDFGPU fix * fixing compilation * warning fix for OdometryFrameImplTMat * docs fix + compile warnings * remake: new class LevMarq with pImpl and enums, LevMarqBase => detail, no Backend class, Settings() => .cpp, Settings==() removed, Settings.set...() inlines * fixing warnings & whitespace
152 lines
4.3 KiB
C++
152 lines
4.3 KiB
C++
#include "perf_precomp.hpp"
|
|
|
|
namespace opencv_test
|
|
{
|
|
using namespace perf;
|
|
|
|
CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)
|
|
|
|
typedef tuple<int, pnpAlgo> PointsNum_Algo_t;
|
|
typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;
|
|
|
|
typedef perf::TestBaseWithParam<int> PointsNum;
|
|
|
|
PERF_TEST_P(PointsNum_Algo, solvePnP,
|
|
testing::Combine( //When non planar, DLT needs at least 6 points for SOLVEPNP_ITERATIVE flag
|
|
testing::Values(6, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
|
|
testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
|
|
)
|
|
)
|
|
{
|
|
int pointsNum = get<0>(GetParam());
|
|
pnpAlgo algo = get<1>(GetParam());
|
|
|
|
vector<Point2f> points2d(pointsNum);
|
|
vector<Point3f> points3d(pointsNum);
|
|
Mat rvec = Mat::zeros(3, 1, CV_32FC1);
|
|
Mat tvec = Mat::zeros(3, 1, CV_32FC1);
|
|
|
|
Mat distortion = Mat::zeros(5, 1, CV_32FC1);
|
|
Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
|
|
intrinsics.at<float> (0, 0) = 400.0;
|
|
intrinsics.at<float> (1, 1) = 400.0;
|
|
intrinsics.at<float> (0, 2) = 640 / 2;
|
|
intrinsics.at<float> (1, 2) = 480 / 2;
|
|
|
|
warmup(points3d, WARMUP_RNG);
|
|
warmup(rvec, WARMUP_RNG);
|
|
warmup(tvec, WARMUP_RNG);
|
|
|
|
projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
|
|
|
|
//add noise
|
|
Mat noise(1, (int)points2d.size(), CV_32FC2);
|
|
randu(noise, 0, 0.01);
|
|
cv::add(points2d, noise, points2d);
|
|
|
|
declare.in(points3d, points2d);
|
|
declare.time(100);
|
|
|
|
TEST_CYCLE_N(1000)
|
|
{
|
|
cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
|
|
}
|
|
|
|
SANITY_CHECK(rvec, 1e-4);
|
|
// the check is relaxed from 1e-4 to 2e-2 after LevMarq replacement
|
|
SANITY_CHECK(tvec, 2e-2);
|
|
}
|
|
|
|
PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
|
|
testing::Combine(
|
|
testing::Values(5),
|
|
testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
|
|
)
|
|
)
|
|
{
|
|
int pointsNum = get<0>(GetParam());
|
|
pnpAlgo algo = get<1>(GetParam());
|
|
if( algo == SOLVEPNP_P3P )
|
|
pointsNum = 4;
|
|
|
|
vector<Point2f> points2d(pointsNum);
|
|
vector<Point3f> points3d(pointsNum);
|
|
Mat rvec = Mat::zeros(3, 1, CV_32FC1);
|
|
Mat tvec = Mat::zeros(3, 1, CV_32FC1);
|
|
|
|
Mat distortion = Mat::zeros(5, 1, CV_32FC1);
|
|
Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
|
|
intrinsics.at<float> (0, 0) = 400.0f;
|
|
intrinsics.at<float> (1, 1) = 400.0f;
|
|
intrinsics.at<float> (0, 2) = 640 / 2;
|
|
intrinsics.at<float> (1, 2) = 480 / 2;
|
|
|
|
warmup(points3d, WARMUP_RNG);
|
|
warmup(rvec, WARMUP_RNG);
|
|
warmup(tvec, WARMUP_RNG);
|
|
|
|
// normalize Rodrigues vector
|
|
Mat rvec_tmp = Mat::eye(3, 3, CV_32F);
|
|
cv::Rodrigues(rvec, rvec_tmp);
|
|
cv::Rodrigues(rvec_tmp, rvec);
|
|
|
|
cv::projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
|
|
|
|
//add noise
|
|
Mat noise(1, (int)points2d.size(), CV_32FC2);
|
|
randu(noise, -0.001, 0.001);
|
|
cv::add(points2d, noise, points2d);
|
|
|
|
declare.in(points3d, points2d);
|
|
declare.time(100);
|
|
|
|
TEST_CYCLE_N(1000)
|
|
{
|
|
cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
|
|
}
|
|
|
|
SANITY_CHECK(rvec, 1e-1);
|
|
SANITY_CHECK(tvec, 1e-2);
|
|
}
|
|
|
|
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
|
|
{
|
|
int count = GetParam();
|
|
|
|
Mat object(1, count, CV_32FC3);
|
|
randu(object, -100, 100);
|
|
|
|
Mat camera_mat(3, 3, CV_32FC1);
|
|
randu(camera_mat, 0.5, 1);
|
|
camera_mat.at<float>(0, 1) = 0.f;
|
|
camera_mat.at<float>(1, 0) = 0.f;
|
|
camera_mat.at<float>(2, 0) = 0.f;
|
|
camera_mat.at<float>(2, 1) = 0.f;
|
|
|
|
Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));
|
|
|
|
vector<cv::Point2f> image_vec;
|
|
|
|
Mat rvec_gold(1, 3, CV_32FC1);
|
|
randu(rvec_gold, 0, 1);
|
|
|
|
Mat tvec_gold(1, 3, CV_32FC1);
|
|
randu(tvec_gold, 0, 1);
|
|
projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);
|
|
|
|
Mat image(1, count, CV_32FC2, &image_vec[0]);
|
|
|
|
Mat rvec;
|
|
Mat tvec;
|
|
|
|
TEST_CYCLE()
|
|
{
|
|
cv::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
|
|
}
|
|
|
|
SANITY_CHECK(rvec, 1e-6);
|
|
SANITY_CHECK(tvec, 1e-6);
|
|
}
|
|
|
|
} // namespace
|