12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed gtsam-opengl convention mismatch

release/4.3a0
Frank Dellaert 2015-07-04 15:54:46 -07:00
parent 8ceaa8a3cc
commit a70815b654
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
timing/timeSFMBAL.cpp
View File

@ -34,7 +34,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
#define USE_GTSAM_FACTOR //#define USE_GTSAM_FACTOR
#ifdef USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
@ -92,13 +92,14 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.number_tracks(); j++) {
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 measurement = m.second; Point2 z = m.second;
#ifdef USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR
graph.push_back(SfmFactor(measurement, unit2, i, P(j))); graph.push_back(SfmFactor(z, unit2, i, P(j)));
#else #else
Expression<Camera> camera_(i); Expression<Camera> camera_(i);
Expression<Point3> point_(P(j)); Expression<Point3> point_(P(j));
graph.addExpressionFactor(unit2, measurement, // Snavely expects measurements in OpenGL format, with y increasing upwards
graph.addExpressionFactor(unit2, Point2(z.x(), -z.y()),
Expression<Point2>(snavely, camera_, point_)); Expression<Point2>(snavely, camera_, point_));
#endif #endif
} }
@ -110,21 +111,23 @@ int main(int argc, char* argv[]) {
#ifdef USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR
initial.insert((i++), camera); initial.insert((i++), camera);
#else #else
Camera ceresCamera(camera.pose(), camera.calibration()); // readBAL converts to GTSAM format, so we need to convert back !
Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration());
initial.insert((i++), ceresCamera); initial.insert((i++), ceresCamera);
#endif #endif
} }
BOOST_FOREACH(const SfM_Track& track, db.tracks) BOOST_FOREACH(const SfM_Track& track, db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
// Check projection // Check projection of first point in first camera
Point2 expected = db.tracks.front().measurements.front().second; Point2 expected = db.tracks.front().measurements.front().second;
Camera camera = initial.at<Camera>(0); Camera camera = initial.at<Camera>(0);
Point3 point = initial.at<Point3>(P(0)); Point3 point = initial.at<Point3>(P(0));
#ifdef USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR
Point2 actual = camera.project(point); Point2 actual = camera.project(point);
#else #else
Point2 actual = snavely(camera, point); Point2 opengl = snavely(camera, point);
Point2 actual(opengl.x(), -opengl.y());
#endif #endif
assert_equal(expected,actual,10); assert_equal(expected,actual,10);