TEST(Resection_Kernel, Multiview) {
const int views_num = 3;
const int points_num = 10;
const NViewDataSet d = NRealisticCamerasRing(views_num, points_num,
NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
const int nResectionCameraIndex = 2;
// Solve the problem and check that fitted value are good enough
{
Mat x = d.projected_points_[nResectionCameraIndex];
Mat X = d.point_3d_;
mvg::multiview::resection::PoseResectionKernel kernel(x, X);
size_t samples_[6]={0,1,2,3,4,5};
std::vector<size_t> samples(samples_, samples_ + 6);
std::vector<Mat34> Ps;
kernel.Fit(samples, &Ps);
for (size_t i = 0; i < x.cols(); ++i) {
EXPECT_NEAR(0.0, kernel.Error(i, Ps[0]), 1e-8);
}
EXPECT_EQ(1, Ps.size());
// Check that Projection matrix is near to the GT :
Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
/ d.P(nResectionCameraIndex).norm();
Mat34 COMPUTED_ProjectionMatrix = Ps[0].array() / Ps[0].norm();
EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-8);
}
}
TEST(Resection_L_Infinity, Robust_OneOutlier) {
const int nViews = 3;
const int nbPoints = 20;
const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints,
nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
d.ExportToPLY("test_Before_Infinity.ply");
//-- Modify a dataset (set to 0 and parse new value) (Assert good values)
NViewDataSet d2 = d;
const int nResectionCameraIndex = 2;
//-- Set to 0 the future computed data to be sure of computation results :
d2._R[nResectionCameraIndex] = Mat3::Zero();
d2._t[nResectionCameraIndex] = Vec3::Zero();
// Set 20% of the 3D point as outlier
const int nbOutlier = nbPoints*0.2;
for (int i=0; i < nbOutlier; ++i)
{
d2._X.col(i)(0) += 120.0;
d2._X.col(i)(1) += -60.0;
d2._X.col(i)(2) += 80.0;
}
// Solve the problem and check that fitted value are good enough
{
typedef lInfinityCV::kernel::l1PoseResectionKernel KernelType;
const Mat & pt2D = d2._x[nResectionCameraIndex];
const Mat & pt3D = d2._X;
KernelType kernel(pt2D, pt3D);
ScorerEvaluator<KernelType> scorer(Square(0.1)); //Highly intolerant for the test
Mat34 P = MaxConsensus(kernel, scorer, NULL, 128);
// Check that Projection matrix is near to the GT :
Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
/ d.P(nResectionCameraIndex).norm();
Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm();
// Extract K[R|t]
Mat3 R,K;
Vec3 t;
KRt_From_P(P, &K, &R, &t);
d2._R[nResectionCameraIndex] = R;
d2._t[nResectionCameraIndex] = t;
//CHeck matrix to GT, and residual
EXPECT_NEAR( 0.0, FrobeniusDistance(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix), 1e-3 );
Mat pt4D = VStack(pt3D, Mat(Vec::Ones(pt3D.cols()).transpose()));
EXPECT_NEAR( 0.0, RootMeanSquareError(pt2D, pt4D, COMPUTED_ProjectionMatrix), 1e-1);
}
d2.ExportToPLY("test_After_Infinity.ply");
}
TEST(Triangulation, TriangulateDLT) {
const NViewDataSet d = NRealisticCamerasRing(2, 12);
for (int i = 0; i < d._X.cols(); ++i) {
const Vec3 X_gt = d._X.col(i);
Vec3 X_estimated;
const Vec2 x1 = d._x[0].col(i);
const Vec2 x2 = d._x[1].col(i);
TriangulateDLT(d.P(0), x1.homogeneous(), d.P(1), x2.homogeneous(), &X_estimated);
EXPECT_NEAR(0, DistanceLInfinity(X_estimated, X_gt), 1e-8);
}
}
TEST(Resection_L_Infinity, Robust_OutlierFree) {
const int nViews = 3;
const int nbPoints = 10;
const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints,
nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
//-- Modify a dataset (set to 0 and parse new value) (Assert good values)
NViewDataSet d2 = d;
const int nResectionCameraIndex = 2;
//-- Set to 0 the future computed data to be sure of computation results :
d2._R[nResectionCameraIndex] = Mat3::Zero();
d2._t[nResectionCameraIndex] = Vec3::Zero();
// Solve the problem and check that fitted value are good enough
{
typedef lInfinityCV::kernel::l1PoseResectionKernel KernelType;
const Mat & pt2D = d2._x[nResectionCameraIndex];
const Mat & pt3D = d2._X;
KernelType kernel(pt2D, pt3D);
ScorerEvaluator<KernelType> scorer(2*Square(0.6));
Mat34 P = MaxConsensus(kernel, scorer, NULL, 128);
// Check that Projection matrix is near to the GT :
Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
/ d.P(nResectionCameraIndex).norm();
Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm();
// Extract K[R|t]
Mat3 R,K;
Vec3 t;
KRt_From_P(P, &K, &R, &t);
d2._R[nResectionCameraIndex] = R;
d2._t[nResectionCameraIndex] = t;
//CHeck matrix to GT, and residual
EXPECT_NEAR( 0.0, FrobeniusDistance(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix), 1e-2 );
Mat pt4D = VStack(pt3D, Mat(Vec::Ones(pt3D.cols()).transpose()));
EXPECT_NEAR( 0.0, RootMeanSquareError(pt2D, pt4D, COMPUTED_ProjectionMatrix), 1e-2);
}
}
TEST(P3P_Kneip_CVPR11, Multiview) {
const int views_num = 3;
const int points_num = 3;
const NViewDataSet d = NRealisticCamerasRing(views_num, points_num,
NViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
const int nResectionCameraIndex = 2;
// Solve the problem and check that fitted value are good enough
{
Mat x = d.projected_points_[nResectionCameraIndex];
Mat X = d.point_3d_;
mvg::multiview::P3P_ResectionKernel_K kernel(x, X, d.camera_matrix_[0]);
size_t samples_[3]={0,1,2};
std::vector<size_t> samples(samples_, samples_+3);
std::vector<Mat34> Ps;
kernel.Fit(samples, &Ps);
bool bFound = false;
char index = -1;
for (size_t i = 0; i < Ps.size(); ++i) {
Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
/ d.P(nResectionCameraIndex).norm();
Mat34 COMPUTED_ProjectionMatrix = Ps[i].array() / Ps[i].norm();
if ( NormLInfinity(GT_ProjectionMatrix - COMPUTED_ProjectionMatrix) < 1e-8 )
{
bFound = true;
index = i;
}
}
EXPECT_TRUE(bFound);
// Check that for the found matrix residual is small
for (size_t i = 0; i < x.cols(); ++i) {
EXPECT_NEAR(0.0, kernel.Error(i,Ps[index]), 1e-8);
}
}
}
NViewDataSet NRealisticCamerasCardioid(size_t nviews, size_t npoints,
const nViewDatasetConfigurator config)
{
//-- Setup a camera circle rig.
NViewDataSet d;
d._n = nviews;
d._K.resize(nviews);
d._R.resize(nviews);
d._t.resize(nviews);
d.C.resize(nviews);
d._x.resize(nviews);
d._x_ids.resize(nviews);
d.X.resize(3, npoints);
d.X.setRandom();
d.X *= 0.6;
Vecu all_point_ids(npoints);
for (size_t j = 0; j < npoints; ++j)
all_point_ids[j] = j;
for (size_t i = 0; i < nviews; ++i) {
Vec3 camera_center, t, jitter, lookdir;
const double theta = i * 2 * M_PI / nviews;
//-- Cardioid
camera_center <<
2*sin(theta)-(sin(2*theta)),
0.0,
2*cos(theta)-(cos(2*theta)); // Y axis UP
camera_center *= config._dist;
d.C[i] = camera_center;
jitter.setRandom();
jitter *= config._jitter_amount / camera_center.norm();
lookdir = -camera_center + jitter;
d._K[i] << config._fx, 0, config._cx,
0, config._fy, config._cy,
0, 0, 1;
d._R[i] = LookAt(lookdir); // Y axis UP
d._t[i] = -d._R[i] * camera_center; // [t]=[-RC] Cf HZ.
d._x[i] = Project(d.P(i), d.X);
d._x_ids[i] = all_point_ids;
}
return d;
}
NViewDataSet NRealisticCamerasCardioid(size_t nviews, size_t npoints,
const NViewDatasetConfigurator config)
{
// 设置相机参数
NViewDataSet d;
d.actual_camera_num_ = nviews;
d.camera_matrix_.resize(nviews);
d.rotation_matrix_.resize(nviews);
d.translation_vector_.resize(nviews);
d.camera_center_.resize(nviews);
d.projected_points_.resize(nviews);
d.projected_point_ids_.resize(nviews);
d.point_3d_.resize(3, npoints);
d.point_3d_.setRandom();
d.point_3d_ *= 0.6;
Vecu all_point_ids(npoints);
for (size_t j = 0; j < npoints; ++j)
all_point_ids[j] = j;
for (size_t i = 0; i < nviews; ++i) {
Vec3 camera_center, t, jitter, lookdir;
const double theta = i * 2 * M_PI / nviews;
// 心形方程式，确定中点
camera_center <<
2 * sin(theta) - (sin(2 * theta)),
0.0,
2 * cos(theta) - (cos(2 * theta)); // Y axis UP
camera_center *= config._dist;
d.camera_center_[i] = camera_center;
jitter.setRandom();
jitter *= config._jitter_amount / camera_center.norm();
lookdir = -camera_center + jitter;
d.camera_matrix_[i] << config._fx, 0, config._cx,
0, config._fy, config._cy,
0, 0, 1;
d.rotation_matrix_[i] = LookAt(lookdir); // Y axis UP
d.translation_vector_[i] = -d.rotation_matrix_[i] * camera_center; // [t]=[-RC] Cf HZ.
d.projected_points_[i] = Project(d.P(i), d.point_3d_);
d.projected_point_ids_[i] = all_point_ids;
}
return d;
}
