diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt new file mode 100644 index 000000000..0c7ecfdf9 --- /dev/null +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -0,0 +1,80 @@ +3 7 19 + +0 0 -385.989990234375 387.1199951171875 +1 0 -38.439998626708984375 492.1199951171875 +2 0 -667.91998291015625 123.1100006103515625 +0 1 383.8800048828125 -15.299989700317382812 +1 1 559.75 -106.15000152587890625 +0 2 591.54998779296875 136.44000244140625 +1 2 863.8599853515625 -23.469970703125 +2 2 494.720001220703125 112.51999664306640625 +0 3 592.5 125.75 +1 3 861.08001708984375 -35.219970703125 +2 3 498.540008544921875 101.55999755859375 +0 4 348.720001220703125 558.3800048828125 +1 4 776.030029296875 483.529998779296875 +2 4 7.7800288200378417969 326.350006103515625 +0 5 14.010009765625 96.420013427734375 +1 5 207.1300048828125 118.3600006103515625 +0 6 202.7599945068359375 340.989990234375 +1 6 543.18011474609375 294.80999755859375 +2 6 -58.419979095458984375 110.8300018310546875 + +-0.016943983733654022217 +0.011171804741024972743 +0.0024643507786095142365 +0.73030996322631824835 +-0.26490819454193109683 +-1.712789297103882058 +1430.031982421875 +-7.5572756941255647689e-08 +3.2377570134516087119e-14 + +0.015049725770950320852 +-0.18504564464092254639 +-0.29278403520584106445 +-1.0590475797653198242 +-0.036017861217260457862 +-1.572034001350402832 +1432.137451171875 +-7.3171918302250560373e-08 +3.1759419042137054801e-14 + +-0.30793598294258112125 +0.32077908515930175781 +0.2225398570299148282 +8.5034484863281267764 +6.7499604225158700288 +-3.6383814811706534087 +1572.047119140625 +-1.5962623223231275915e-08 +-1.6507904730136101212e-14 + +-12.055994987487792969 +12.838775634765625 +-41.099369049072265625 + +6.4168906211853027344 +0.38897031545639038086 +-23.586282730102539062 + +13.051100730895996094 +3.8387587070465087891 +-29.777933120727539062 + +13.060946464538574219 +3.5910520553588867188 +-29.759080886840820312 + +14.265764236450195312 +24.096216201782226562 +-54.823970794677734375 + +-0.25292283296585083008 +2.2166082859039306641 +-21.712127685546875 + +7.6465740203857421875 +14.185332298278808594 +-52.070301055908203125 + diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 269b3f0b4..a9817a8ea 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -229,7 +229,7 @@ pair load2D( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -403,24 +403,45 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } +/* ************************************************************************* */ +Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL + /* R = [ 1 0 0 + * 0 -1 0 + * 0 0 -1] + */ + Matrix3 R_mat = Matrix3::Zero(3,3); + R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; + return Rot3(R_mat); +} + /* ************************************************************************* */ Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) { -// Our camera-to-world rotation matrix wRc' = [e1 -e2 -e3] * R -Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! -Rot3 cRw( - r1.x(), r2.x(), r3.x(), - -r1.y(), -r2.y(), -r3.y(), - -r1.z(), -r2.z(), -r3.z()); -Rot3 wRc = cRw.inverse(); + Rot3 R90 = openGLFixedRotation(); + Rot3 wRc = ( R.inverse() ).compose(R90); -// Our camera-to-world translation wTc = -R'*t -return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); + // Our camera-to-world translation wTc = -R'*t + return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) +{ + Rot3 R90 = openGLFixedRotation(); + Rot3 cRw_openGL = R90.compose( R.inverse() ); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz)); + return Pose3(cRw_openGL, t_openGL); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Pose3& PoseGTSAM) +{ + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); } /* ************************************************************************* */ @@ -581,5 +602,63 @@ bool readBAL(const string& filename, SfM_data &data) return true; } +/* ************************************************************************* */ +bool writeBAL(const string& filename, SfM_data &data) +{ + // Load the data file + ofstream os; + os.open(filename.c_str()); + os.precision(20); + if (!os.is_open()) { + cout << "Error in writeBAL: can not open the file!!" << endl; + return false; + } + + // Write the number of camera poses and 3D points + int nrObservations=0; + for (size_t j = 0; j < data.number_tracks(); j++){ + nrObservations += data.tracks[j].number_measurements(); + } + + // Write observations + os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; + os << endl; + + for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + SfM_Track track = data.tracks[j]; + for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + Point2 pixelMeasurement(track.measurements[k].second.x(), -track.measurements[k].second.y()); + os << track.measurements[k].first /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; + } + } + os << endl; + + // Write cameras + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Pose3 poseGTSAM = data.cameras[i].pose(); + Cal3Bundler cameraCalibration = data.cameras[i].calibration(); + Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; + os << endl; + } + + // Write the points + for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j + Point3 point = data.tracks[j].p; + os << point.x() << endl; + os << point.y() << endl; + os << point.z() << endl; + os << endl; + } + + os.close(); + return true; +} + } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 522a5f799..252450c5d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -126,4 +126,40 @@ bool readBundler(const std::string& filename, SfM_data &data); */ bool readBAL(const std::string& filename, SfM_data &data); +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a + * SfM_data structure + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +bool writeBAL(const std::string& filename, SfM_data &data); + +/** + * @brief This function converts an openGL camera pose to an GTSAM camera pose + * @param R rotation in openGL + * @param tx x component of the translation in openGL + * @param ty y component of the translation in openGL + * @param tz z component of the translation in openGL + * @return Pose3 in GTSAM format + */ +Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param R rotation in GTSAM + * @param tx x component of the translation in GTSAM + * @param ty y component of the translation in GTSAM + * @param tz z component of the translation in GTSAM + * @return Pose3 in openGL format + */ +Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param PoseGTSAM pose in GTSAM format + * @return Pose3 in openGL format + */ +Pose3 gtsam2openGL(const Pose3& PoseGTSAM); + } // namespace gtsam diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index a5ebbc167..79311fa92 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -56,7 +56,7 @@ TEST( dataSet, Balbianello) } /* ************************************************************************* */ -TEST( dataSet, Dubrovnik) +TEST( dataSet, readBAL_Dubrovnik) { ///< The structure where we will save the SfM data const string filename = findExampleDataFile("dubrovnik-3-7-pre"); @@ -76,6 +76,87 @@ TEST( dataSet, Dubrovnik) EXPECT(assert_equal(expected,actual,12)); } +/* ************************************************************************* */ +TEST( dataSet, openGL2gtsam) +{ + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(0.0,0.0,0.0); + Pose3 poseGTSAM = Pose3(R,t); + + Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! + Rot3 cRw( + r1.x(), r2.x(), r3.x(), + -r1.y(), -r2.y(), -r3.y(), + -r1.z(), -r2.z(), -r3.z()); + Rot3 wRc = cRw.inverse(); + Pose3 actual = Pose3(wRc,t); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( dataSet, gtsam2openGL) +{ + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(1.0,20.0,10.0); + Pose3 actual = Pose3(R,t); + Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Pose3 expected = gtsam2openGL(poseGTSAM); + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( dataSet, writeBAL_Dubrovnik) +{ + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data readData; + CHECK(readBAL(filenameToRead, readData)); + + // Write readData to file filenameToWrite + const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + CHECK(writeBAL(filenameToWrite, readData)); + + // Read what we wrote + SfM_data writtenData; + CHECK(readBAL(filenameToWrite, writtenData)); + + // Check that what we read is the same as what we wrote + EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras())); + EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks())); + + for (size_t i = 0; i < readData.number_cameras(); i++){ + PinholeCamera expectedCamera = writtenData.cameras[i]; + PinholeCamera actualCamera = readData.cameras[i]; + EXPECT(assert_equal(expectedCamera,actualCamera)); + } + + for (size_t j = 0; j < readData.number_tracks(); j++){ + // check point + SfM_Track expectedTrack = writtenData.tracks[j]; + SfM_Track actualTrack = readData.tracks[j]; + Point3 expectedPoint = expectedTrack.p; + Point3 actualPoint = actualTrack.p; + EXPECT(assert_equal(expectedPoint,actualPoint)); + + // check rgb + Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b ); + Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b); + EXPECT(assert_equal(expectedRGB,actualRGB)); + + // check measurements + for (size_t k = 0; k < actualTrack.number_measurements(); k++){ + EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first)); + EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); + } + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */