From 93a3f4892059504b75303e71ec5160ac04a1e89a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 25 Feb 2010 00:05:27 +0000
Subject: [PATCH] Created patch to camera homography function with Duy

---
 cpp/tensorInterface.h   | 15 ++++++-
 cpp/testHomography2.cpp | 93 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 106 insertions(+), 2 deletions(-)

diff --git a/cpp/tensorInterface.h b/cpp/tensorInterface.h
index 0f41ce365..c3d2b7018 100644
--- a/cpp/tensorInterface.h
+++ b/cpp/tensorInterface.h
@@ -10,10 +10,10 @@
 
 namespace gtsam {
 
-	/** Reshape 3*3 rank 2 tensor into Matrix */
+	/** Reshape rank 2 tensor into Matrix */
 	template<class A, class I, class J>
 	Matrix reshape(const tensors::Tensor2Expression<A, I, J>& T, int m, int n) {
-		if (m * n != 9) throw std::invalid_argument(
+		if (m * n != I::dim * J::dim) throw std::invalid_argument(
 				"reshape: incompatible dimensions");
 		Matrix M(m, n);
 		size_t t = 0;
@@ -23,6 +23,17 @@ namespace gtsam {
 		return M;
 	}
 
+	/** Reshape 3*3 rank 2 tensor into Vector : TODO 9 ???*/
+	template<class A, class I, class J>
+	Vector toVector(const tensors::Tensor2Expression<A, I, J>& T) {
+		Vector v(I::dim * J::dim);
+		size_t t = 0;
+		for (int j = 0; j < J::dim; j++)
+			for (int i = 0; i < I::dim; i++)
+				v(t++) = T(i, j);
+		return v;
+	}
+
 	/** Reshape Vector into rank 2 tensor */
 	template<int N1, int N2>
 	tensors::Tensor2<N1, N2> reshape2(const Vector& v) {
diff --git a/cpp/testHomography2.cpp b/cpp/testHomography2.cpp
index decc1922b..9a45c9799 100644
--- a/cpp/testHomography2.cpp
+++ b/cpp/testHomography2.cpp
@@ -12,9 +12,11 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include "Testable.h"
 #include "tensors.h"
 #include "tensorInterface.h"
 #include "projectiveGeometry.h"
+#include "Pose3.h"
 
 using namespace std;
 using namespace gtsam;
@@ -95,6 +97,97 @@ TEST( Homography2, EstimateReverse)
 	CHECK(assert_equality(reverse(_a,b),estimatedH(_a,b)*(1.0/estimatedH(2,2))));
 }
 
+/* ************************************************************************* */
+/**
+ * Computes the homography H(I,_T) from template to image
+ * given the pose tEc of the camera in the template coordinate frame.
+ * Assumption is Z is normal to the template, template texture in X-Y plane.
+ */
+Homography2 patchH(const Pose3& tEc) {
+	Pose3 cEt = inverse(tEc);
+	Rot3 cRt = cEt.rotation();
+	Point3 r1 = cRt.column(1), r2 = cRt.column(2), t = cEt.translation();
+
+	// TODO cleanup !!!!
+	// column 1
+	double H11 = r1.x();
+	double H21 = r1.y();
+	double H31 = r1.z();
+	// column 2
+	double H12 = r2.x();
+	double H22 = r2.y();
+	double H32 = r2.z();
+	// column 3
+	double H13 = t.x();
+	double H23 = t.y();
+	double H33 = t.z();
+	double data2[3][3] = { { H11, H21, H31 }, { H12, H22, H32 },
+			{ H13, H23, H33 } };
+	return Homography2(data2);
+}
+
+/* ************************************************************************* */
+namespace gtsam {
+	size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
+	Vector toVector(const tensors::Tensor2<3, 3>& H) {
+		Index<3, 'T'> _T; // covariant 2D template
+		Index<3, 'C'> I; // contravariant 2D camera
+		return toVector(H(I,_T));
+	}
+	Vector logmap(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
+		return toVector(A)-toVector(B); // TODO correct order ?
+	}
+}
+
+#include "numericalDerivative.h"
+
+/* ************************************************************************* */
+TEST( Homography2, patchH)
+{
+	Index<3, 'T'> _T; // covariant 2D template
+	Index<3, 'C'> I; // contravariant 2D camera
+
+	// data[_T][I]
+	double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
+	Homography2 expected(data1);
+
+	// camera rotation, looking in negative Z
+	Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
+	Point3 gTc(0,0,10); // Camera location, out on the Z axis
+	Pose3 gEc(gRc,gTc); // Camera pose
+
+	Homography2 actual = patchH(gEc);
+
+//	GTSAM_PRINT(expected(I,_T));
+//	GTSAM_PRINT(actual(I,_T));
+	CHECK(assert_equality(expected(I,_T),actual(I,_T)));
+
+  Matrix D = numericalDerivative11<Homography2,Pose3>(patchH, gEc);
+//  print(D,"D");
+}
+
+/* ************************************************************************* */
+TEST( Homography2, patchH2)
+{
+	Index<3, 'T'> _T; // covariant 2D template
+	Index<3, 'C'> I; // contravariant 2D camera
+
+	// data[_T][I]
+	double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
+	Homography2 expected(data1);
+
+	// camera rotation, looking in negative Z
+	Rot3 gRc(Point3(1,0,0),Point3(0,-1,0),Point3(0,0,-1));
+	Point3 gTc(0,0,10); // Camera location, out on the Z axis
+	Pose3 gEc(gRc,gTc); // Camera pose
+
+	Homography2 actual = patchH(gEc);
+
+//	GTSAM_PRINT(expected(I,_T));
+//	GTSAM_PRINT(actual(I,_T));
+	CHECK(assert_equality(expected(I,_T),actual(I,_T)));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;