diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index f42510b4a..baa515029 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -25,6 +25,7 @@ #include #include +#include // std::setprecision using namespace std; using namespace gtsam; @@ -39,9 +40,18 @@ void time(const NonlinearFactor& f, const Values& values) { long timeLog2 = clock(); double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC; // cout << ((double) n / seconds) << " calls/second" << endl; + cout << setprecision(3); cout << ((double) seconds * 1000000 / n) << " musecs/call" << endl; } +boost::shared_ptr fixedK(new Cal3_S2()); + +Point2 myProject(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) { + PinholeCamera camera(pose, *fixedK); + return camera.project(point, H1, H2); +} + int main() { // Create leaves @@ -63,33 +73,38 @@ int main() { // Dedicated factor // Oct 3, 2014, Macbook Air - // 4.44887 musecs/call + // 4.2 musecs/call GeneralSFMFactor2 oldFactor2(z, model, 1, 2, 3); time(oldFactor2, values); // BADFactor // Oct 3, 2014, Macbook Air - // 20.7554 musecs/call + // 20.3 musecs/call BADFactor newFactor2(model, z, uncalibrate(K, project(transform_to(x, p)))); time(newFactor2, values); // CALIBRATED - boost::shared_ptr fixedK(new Cal3_S2()); - // Dedicated factor // Oct 3, 2014, Macbook Air - // 3.69707 musecs/call + // 3.4 musecs/call GenericProjectionFactor oldFactor1(z, model, 1, 2, fixedK); time(oldFactor1, values); // BADFactor // Oct 3, 2014, Macbook Air - // 17.092 musecs/call + // 16.0 musecs/call BADFactor newFactor1(model, z, uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); time(newFactor1, values); + // BADFactor, optimized + // Oct 3, 2014, Macbook Air + // 9.0 musecs/call + typedef PinholeCamera Camera; + typedef Expression Camera_; + BADFactor newFactor3(model, z, Point2_(myProject, x, p)); + time(newFactor3, values); return 0; }