C++ Transform3D::getTransformationMatrix方法代码示例

    GenerateTestData()
    {
        // Instantiate a pinhole camera model.

        PinholeCameraModel<double> camera_model;

        // Set the intrinsic camera parameters.

        camera_model.setFocalLengths(80, 79);
        camera_model.setImageCenter(250, 251);
        camera_model.setSkew(0.1);

        // Set the extrinsic camera parameters.

        Transform3D<double> T;
        T.rotateAxis(40, Point3D(1, 2, 3), Transform3D<double>::DEGREES);
        T.translate(130, 135, 90);
        camera_model.setExtrinsicParameters(T.getTransformationMatrix());

        // Generate some test data.

        const int number_of_points = 6;

        for (int i = 0; i < number_of_points; ++i) // generate points in the world COS
        {
            auto x = randn(0.0, 30.0);    // in the camera COS
            auto y = randn(0.0, 30.0);    // in the camera COS
            auto z = randn(600.0, 10.0);  // in the camera COS
            Point3D p = T.inverse() * Point3D(x, y, z);  // in the world COS
            points_world.emplace_back(p);
        }

        for (auto const & p_W : points_world) // generate points in the camera COS
        {
            Point2D p_D = camera_model.transform(p_W);
            points_display.push_back(p_D);
        }
    }

void unit_test_EstimateSimilarityTransformation3D()
{
    using std::cout;
    using std::endl;
    using std::vector;

    using TRTK::Tools::isEqual;

    using namespace TRTK;


    typedef EstimateSimilarityTransformation3D<double> EstimateSimilarityTransformation;
    typedef EstimateSimilarityTransformation::value_type T;
    typedef EstimateSimilarityTransformation::Matrix3T Matrix3T;
    typedef EstimateSimilarityTransformation::Matrix4T Matrix4T;
    typedef EstimateSimilarityTransformation::Vector3T Vector3T;
    typedef EstimateSimilarityTransformation::Vector4T Vector4T;


    // Construct a transformation which rotates 90 degrees counter-clockwise in
    // the x-y plane with a center of rotation of (1, 3, 0) and a scaling of 2.

    Transform3D<T> transform;
    const T pi = Transform3D<T>::pi;
    transform.translate(-1, -3, 0).rotateZ(pi/2).scale(2, 2, 2).translate(1, 3, 0);


    // Construct sets with source points.

    vector<Coordinate<T> >                                 source_points1;
    vector<Vector3T>                                       source_points2;
    vector<Vector4T, Eigen::aligned_allocator<Vector4T> >  source_points3;

    Coordinate<T> source_point1( 1,  2,  0);
    Coordinate<T> source_point2( 3, -2,  0);
    Coordinate<T> source_point3(-1,  1,  1);
    Coordinate<T> source_point4( 2,  0, -1);

    source_points1.push_back(source_point1);
    source_points1.push_back(source_point2);
    source_points1.push_back(source_point3);
    source_points1.push_back(source_point4);

    source_points2.push_back(source_point1.toArray());
    source_points2.push_back(source_point2.toArray());
    source_points2.push_back(source_point3.toArray());
    source_points2.push_back(source_point4.toArray());

    source_points3.push_back((source_point1, 1).toArray());
    source_points3.push_back((source_point2, 1).toArray());
    source_points3.push_back((source_point3, 1).toArray());
    source_points3.push_back((source_point4, 1).toArray());


    // Construct sets with corresponding target points.

    vector<Coordinate<T> >                                 target_points1;
    vector<Vector3T>                                       target_points2;
    vector<Vector4T, Eigen::aligned_allocator<Vector4T> >  target_points3;

    Coordinate<T> target_point1 = transform * source_point1;
    Coordinate<T> target_point2 = transform * source_point2;
    Coordinate<T> target_point3 = transform * source_point3;
    Coordinate<T> target_point4 = transform * source_point4;

    target_points1.push_back(target_point1);
    target_points1.push_back(target_point2);
    target_points1.push_back(target_point3);
    target_points1.push_back(target_point4);

    target_points2.push_back(target_point1.toArray());
    target_points2.push_back(target_point2.toArray());
    target_points2.push_back(target_point3.toArray());
    target_points2.push_back(target_point4.toArray());

    target_points3.push_back((target_point1, 1).toArray());
    target_points3.push_back((target_point2, 1).toArray());
    target_points3.push_back((target_point3, 1).toArray());
    target_points3.push_back((target_point4, 1).toArray());


    HEADING(EstimateSimilarityTransformation3D<T>)


    SUBHEADING(Constructors)


        // Empty constructor.

        START_TEST
            EstimateSimilarityTransformation estimateSimilarityTransformation;
            assert(estimateSimilarityTransformation.getTransformationMatrix().isApprox(Matrix4T::Identity()));
            assert(estimateSimilarityTransformation.getRotationMatrix().isApprox(Matrix3T::Identity()));
            assert(estimateSimilarityTransformation.getTranslationVector().isApprox(Vector3T::Zero()));
        STOP_TEST


        // Constructor with vector<Coordinate>.

        START_TEST
//.........这里部分代码省略.........