added writeBAL utility and pose conversion between GTSAM and openCV camera conventions
Luca Carlone
parent
f7e90bbe86
commit
52a1b3d444
|
|
@ -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
|
||||
|
||||
|
|
@ -229,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> 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<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> 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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<Cal3Bundler> expectedCamera = writtenData.cameras[i];
|
||||
PinholeCamera<Cal3Bundler> 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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue