C++ NViewDataSet::P方法代码示例

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);
  }
}

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;
		}