示例#1
0
  SimpleCamera simpleCamera(const Matrix& P) {

    // P = [A|a] = s K cRw [I|-T], with s the unknown scale
    Matrix A = P.topLeftCorner(3, 3);
    Vector a = P.col(3);

    // do RQ decomposition to get s*K and cRw angles
    Matrix sK;
    Vector xyz;
    boost::tie(sK, xyz) = RQ(A);

    // Recover scale factor s and K
    double s = sK(2, 2);
    Matrix K = sK / s;

    // Recover cRw itself, and its inverse
    Rot3 cRw = Rot3::RzRyRx(xyz);
    Rot3 wRc = cRw.inverse();

    // Now, recover T from a = - s K cRw T = - A T
    Vector T = -(A.inverse() * a);
    return SimpleCamera(Pose3(wRc, T),
        Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
  }
namespace example1 {

const string filename = findExampleDataFile("5pointExample1.txt");
SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
double baseline = 0.1; // actual baseline of the camera

Point2 pA(size_t i) {
    return data.tracks[i].measurements[0].second;
}
Point2 pB(size_t i) {
    return data.tracks[i].measurements[1].second;
}
Vector vA(size_t i) {
    return EssentialMatrix::Homogeneous(pA(i));
}
Vector vB(size_t i) {
    return EssentialMatrix::Homogeneous(pB(i));
}

//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
    CHECK(readOK);

    // Check E matrix
    Matrix expected(3, 3);
    expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
    Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
                        * c1Rc2.matrix();
    EXPECT(assert_equal(expected, aEb_matrix, 1e-8));

    // Check some projections
    EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
    EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
    EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
    EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));

    // Check homogeneous version
    EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));

    // Check epipolar constraint
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);

    // Check epipolar constraint
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}

//*************************************************************************
TEST (EssentialMatrixFactor, factor) {
    for (size_t i = 0; i < 5; i++) {
        EssentialMatrixFactor factor(1, pA(i), pB(i), model1);

        // Check evaluation
        Vector expected(1);
        expected << 0;
        Matrix Hactual;
        Vector actual = factor.evaluateError(trueE, Hactual);
        EXPECT(assert_equal(expected, actual, 1e-7));

        // Use numerical derivatives to calculate the expected Jacobian
        Matrix Hexpected;
        typedef Eigen::Matrix<double,1,1> Vector1;
        Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
                        boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
                                    boost::none), trueE);

        // Verify the Jacobian is correct
        EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
    }
}

//*************************************************************************
TEST (EssentialMatrixFactor, minimization) {
    // Here we want to optimize directly on essential matrix constraints
    // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
    // but GTSAM does the equivalent anyway, provided we give the right
    // factors. In this case, the factors are the constraints.

    // We start with a factor graph and add constraints to it
    // Noise sigma is 1cm, assuming metric measurements
    NonlinearFactorGraph graph;
    for (size_t i = 0; i < 5; i++)
        graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));

    // Check error at ground truth
    Values truth;
    truth.insert(1, trueE);
    EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

    // Check error at initial estimate
    Values initial;
    EssentialMatrix initialE = trueE.retract(
                                   (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
    initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
    EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
    EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif

    // Optimize
    LevenbergMarquardtParams parameters;
    LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
    Values result = optimizer.optimize();

    // Check result
    EssentialMatrix actual = result.at<EssentialMatrix>(1);
    EXPECT(assert_equal(trueE, actual, 1e-1));

    // Check error at result
    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);

    // Check errors individually
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);

}

//*************************************************************************
TEST (EssentialMatrixFactor2, factor) {
    for (size_t i = 0; i < 5; i++) {
        EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);

        // Check evaluation
        Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
        const Point2 pi = PinholeBase::Project(P2);
        Point2 reprojectionError(pi - pB(i));
        Vector expected = reprojectionError.vector();

        Matrix Hactual1, Hactual2;
        double d(baseline / P1.z());
        Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
        EXPECT(assert_equal(expected, actual, 1e-7));

        // Use numerical derivatives to calculate the expected Jacobian
        Matrix Hexpected1, Hexpected2;
        boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
                    &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
                    boost::none);
        Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
        Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);

        // Verify the Jacobian is correct
        EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
        EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
    }
}

//*************************************************************************
TEST (EssentialMatrixFactor2, minimization) {
    // Here we want to optimize for E and inverse depths at the same time

    // We start with a factor graph and add constraints to it
    // Noise sigma is 1cm, assuming metric measurements
    NonlinearFactorGraph graph;
    for (size_t i = 0; i < 5; i++)
        graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));

    // Check error at ground truth
    Values truth;
    truth.insert(100, trueE);
    for (size_t i = 0; i < 5; i++) {
        Point3 P1 = data.tracks[i].p;
        truth.insert(i, double(baseline / P1.z()));
    }
    EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

    // Optimize
    LevenbergMarquardtParams parameters;
    // parameters.setVerbosity("ERROR");
    LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
    Values result = optimizer.optimize();

    // Check result
    EssentialMatrix actual = result.at<EssentialMatrix>(100);
    EXPECT(assert_equal(trueE, actual, 1e-1));
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);

    // Check error at result
    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

//*************************************************************************
// Below we want to optimize for an essential matrix specified in a
// body coordinate frame B which is rotated with respect to the camera
// frame C, via the rotation bRc.

// The "true E" in the body frame is then
EssentialMatrix bodyE = cRb.inverse() * trueE;

//*************************************************************************
TEST (EssentialMatrixFactor3, factor) {

    for (size_t i = 0; i < 5; i++) {
        EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);

        // Check evaluation
        Point3 P1 = data.tracks[i].p;
        const Point2 pi = camera2.project(P1);
        Point2 reprojectionError(pi - pB(i));
        Vector expected = reprojectionError.vector();

        Matrix Hactual1, Hactual2;
        double d(baseline / P1.z());
        Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
        EXPECT(assert_equal(expected, actual, 1e-7));

        // Use numerical derivatives to calculate the expected Jacobian
        Matrix Hexpected1, Hexpected2;
        boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
                    &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
                    boost::none);
        Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
        Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);

        // Verify the Jacobian is correct
        EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
        EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
    }
}

//*************************************************************************
TEST (EssentialMatrixFactor3, minimization) {

    // As before, we start with a factor graph and add constraints to it
    NonlinearFactorGraph graph;
    for (size_t i = 0; i < 5; i++)
        // but now we specify the rotation bRc
        graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));

    // Check error at ground truth
    Values truth;
    truth.insert(100, bodyE);
    for (size_t i = 0; i < 5; i++) {
        Point3 P1 = data.tracks[i].p;
        truth.insert(i, double(baseline / P1.z()));
    }
    EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

    // Optimize
    LevenbergMarquardtParams parameters;
    // parameters.setVerbosity("ERROR");
    LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
    Values result = optimizer.optimize();

    // Check result
    EssentialMatrix actual = result.at<EssentialMatrix>(100);
    EXPECT(assert_equal(bodyE, actual, 1e-1));
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);

    // Check error at result
    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

} // namespace example1