diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index b62b1c6fa..bcc0b6320 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -17,28 +17,29 @@ #include #include + +#include #include using namespace boost::assign; using namespace std; using namespace gtsam; -void create5PointExample1() { +/* ************************************************************************* */ + +void createExampleBALFile(const string& filename, const vector& P, + const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = + Cal3Bundler()) { // Class that will gather all data SfM_data data; - // Create two cameras and corresponding essential matrix E + // Create two cameras Rot3 aRb = Rot3::yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 identity, aPb(aRb, aTb); - data.cameras.push_back(SfM_Camera(identity)); - data.cameras.push_back(SfM_Camera(aPb)); - - // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + data.cameras.push_back(SfM_Camera(pose1, K)); + data.cameras.push_back(SfM_Camera(pose2, K)); BOOST_FOREACH(const Point3& p, P) { @@ -51,19 +52,59 @@ void create5PointExample1() { // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); } - // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample1.txt"; writeBAL(filename, data); } +/* ************************************************************************* */ + +void create5PointExample1() { + + // Create two cameras poses + Rot3 aRb = Rot3::yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need at least 5 points + vector P; + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/5pointExample1.txt"; + createExampleBALFile(filename, P, pose1, pose2); +} + +/* ************************************************************************* */ + +void create5PointExample2() { + + // Create two cameras poses + Rot3 aRb = Rot3::yaw(M_PI_2); + Point3 aTb(10, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need at least 5 points + vector P; + P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/5pointExample2.txt"; + Cal3Bundler K(500, 0, 0); + createExampleBALFile(filename, P, pose1, pose2,K); +} + +/* ************************************************************************* */ + int main(int argc, char* argv[]) { create5PointExample1(); + create5PointExample2(); return 0; } diff --git a/examples/Data/5pointExample2.txt b/examples/Data/5pointExample2.txt new file mode 100644 index 000000000..1aa57d59d --- /dev/null +++ b/examples/Data/5pointExample2.txt @@ -0,0 +1,65 @@ +2 7 14 + +0 0 0 -0 +1 0 -3.0616169978683830426e-15 -50 +0 1 -50 -0 +1 1 -6.1232339957367660852e-15 -100 +0 2 50 -0 +1 2 0 -0 +0 3 0 -500 +1 3 500 -100.00000000000002842 +0 4 0 500 +1 4 -500 -99.999999999999957367 +0 5 -125 -0 +1 5 -1.1481063742006437494e-14 -187.5 +0 6 125 312.5 +1 6 -312.5 62.500000000000028422 + +3.141592653589793116 +0 +0 +-0 +0 +0 +500 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 +0 +-6.1232339957367662824e-16 +-10 +0 +500 +0 +0 + +0 +0 +100 + +-10 +0 +100 + +10 +0 +100 + +0 +50 +50 + +0 +-50 +50 + +-20 +0 +80 + +20 +-50 +80 + diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 090a816ec..362fac619 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -66,7 +66,8 @@ public: class EssentialMatrixFactor2: public NoiseModelFactor2 { - Point2 pA_, pB_; ///< Measurements in image A and B + Point3 dP1_; ///< 3D point corresponding to measurement in image 1 + Point2 p1_, p2_; ///< Measurements in image 1 and image 2 Cal3_S2 K_; ///< Calibration typedef NoiseModelFactor2 Base; @@ -77,7 +78,9 @@ public: /// Constructor EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const Cal3_S2& K, const SharedNoiseModel& model) : - Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) { + Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) { + Point2 xy = K_.calibrate(p1_); + dP1_ = Point3(xy.x(), xy.y(), 1); } /// @return a deep copy of this factor @@ -91,7 +94,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() + << p1_.vector().transpose() << ")' and (" << p2_.vector().transpose() << ")'" << std::endl; } @@ -102,11 +105,11 @@ public: // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d - // We then convert to first camera by 2P = 1R2Õ*(P1-1T2) + // We then convert to first camera by 2P = 1R2�*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) // Note that this is just a homography for d==0 - Point3 dP1(pA_.x(), pA_.y(), 1); + // The point d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate Point2 pi; @@ -114,7 +117,7 @@ public: Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1 - d1T2); + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); Point2 pn = SimpleCamera::project_to_camera(dP2); pi = K_.uncalibrate(pn); @@ -126,7 +129,7 @@ public: Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); pi = K_.uncalibrate(pn, boost::none, Dpi_pn); @@ -141,7 +144,7 @@ public: *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); } - Point2 reprojectionError = pi - pB_; + Point2 reprojectionError = pi - p2_; return reprojectionError.vector(); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 57fb47715..5f9233bb3 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -20,6 +20,8 @@ using namespace gtsam; typedef noiseModel::Isotropic::shared_ptr Model; +namespace example1 { + const string filename = findExampleDataFile("5pointExample1.txt"); SfM_data data; bool readOK = readBAL(filename, data); @@ -207,12 +209,74 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual,1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(result.at(i), truth.at(i),1e-1)); + EXPECT(assert_equal(truth.at(i),result.at(i),1e-1)); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } +} // namespace example1 + +//************************************************************************* + +namespace example2 { + +const string filename = findExampleDataFile("5pointExample2.txt"); +SfM_data data; +bool readOK = readBAL(filename, data); +Rot3 aRb = data.cameras[1].pose().rotation(); +Point3 aTb = data.cameras[1].pose().translation(); +double baseline = 10; // 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; +} + +// Matches Cal3Bundler K(500, 0, 0); +Cal3_S2 K(500, 500, 0, 0, 0); + +//************************************************************************* +TEST (EssentialMatrixFactor2, extraTest) { + // Additional test with camera moving in positive X direction + + // We start with a factor graph and add constraints to it + // Noise sigma is 1, assuming pixel measurements + NonlinearFactorGraph graph; + Model model = noiseModel::Isotropic::Sigma(2, 1); + for (size_t i = 0; i < data.number_tracks(); i++) + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model)); + + // Check error at ground truth + Values truth; + EssentialMatrix trueE(aRb, aTb); + truth.insert(100, trueE); + for (size_t i = 0; i < data.number_tracks(); i++) { + Point3 P1 = data.tracks[i].p; + truth.insert(i, LieScalar(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(100); + EXPECT(assert_equal(trueE, actual,1e-1)); + for (size_t i = 0; i < data.number_tracks(); i++) + EXPECT(assert_equal(truth.at(i),result.at(i),1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + +} + /* ************************************************************************* */ int main() { TestResult tr;