From 0f53c8d5ec01b35218b6194ce958ac3e66db44a3 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 22 Oct 2014 12:49:18 +0200
Subject: [PATCH] Timing of Ceres AutoDiff adaptor

---
 .cproject                                   |  8 +++
 gtsam_unstable/nonlinear/ceres_rotation.h   |  1 +
 gtsam_unstable/timing/timeAdaptAutoDiff.cpp | 73 +++++++++++++++++++++
 3 files changed, 82 insertions(+)
 create mode 100644 gtsam_unstable/timing/timeAdaptAutoDiff.cpp

diff --git a/.cproject b/.cproject
index 895e2667a..45febcf1b 100644
--- a/.cproject
+++ b/.cproject
@@ -982,6 +982,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="timeAdaptAutoDiff.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timeAdaptAutoDiff.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam_unstable/nonlinear/ceres_rotation.h
index 896761296..83627291c 100644
--- a/gtsam_unstable/nonlinear/ceres_rotation.h
+++ b/gtsam_unstable/nonlinear/ceres_rotation.h
@@ -47,6 +47,7 @@
 
 #include <algorithm>
 #include <cmath>
+#define DCHECK assert
 
 namespace ceres {
 
diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp
new file mode 100644
index 000000000..c4ea7a8f3
--- /dev/null
+++ b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp
@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    timeCameraExpression.cpp
+ * @brief   time CalibratedCamera derivatives
+ * @author  Frank Dellaert
+ * @date    October 3, 2014
+ */
+
+#include "timeLinearize.h"
+#include <gtsam_unstable/nonlinear/ceres_example.h>
+#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Point3.h>
+
+using namespace std;
+using namespace gtsam;
+
+#define time timeMultiThreaded
+
+int main() {
+
+  // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
+  typedef PinholeCamera<Cal3Bundler> Camera;
+  typedef Expression<Point2> Point2_;
+  typedef Expression<Camera> Camera_;
+  typedef Expression<Point3> Point3_;
+
+  // Create leaves
+  Camera_ camera(1);
+  Point3_ point(2);
+
+  // Some parameters needed
+  Point2 z(-17, 30);
+  SharedNoiseModel model = noiseModel::Unit::Create(2);
+
+  // Create values
+  Values values;
+  values.insert(1, Camera());
+  values.insert(2, Point3(0, 0, 1));
+
+  // Dedicated factor
+  NonlinearFactor::shared_ptr f1 = boost::make_shared<
+      GeneralSFMFactor<Camera, Point3> >(z, model, 1, 2);
+  time("GeneralSFMFactor<Camera>                : ", f1, values);
+
+  // AdaptAutoDiff
+  typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> SnavelyAdaptor;
+  NonlinearFactor::shared_ptr f2 =
+      boost::make_shared<ExpressionFactor<Point2> >(model, z,
+          Point2_(SnavelyAdaptor(), camera, point));
+  time("Point2_(SnavelyAdaptor(), camera, point): ", f2, values);
+
+  // ExpressionFactor
+  NonlinearFactor::shared_ptr f3 =
+      boost::make_shared<ExpressionFactor<Point2> >(model, z,
+          Point2_(camera, &Camera::project2, point));
+  time("Point2_(camera, &Camera::project, point): ", f3, values);
+
+  return 0;
+}