From f063308bbcde49e7d84bdf3ef0b4da491f9fb5a3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 6 Jan 2012 01:26:52 +0000
Subject: [PATCH] Cayley transform

---
 gtsam/base/Matrix.cpp | 27 +++++++++++++---------
 gtsam/base/Matrix.h   | 52 ++++++++++++++++++++++---------------------
 2 files changed, 44 insertions(+), 35 deletions(-)

diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index c20521c95..b1940e335 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -15,6 +15,16 @@
  * @author Christian Potthast
  */
 
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/Vector.h>
+
+#include <gtsam/3rdparty/Eigen/Eigen/Dense>
+#include <gtsam/3rdparty/Eigen/Eigen/SVD>
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+
 #include <stdarg.h>
 #include <string.h>
 #include <iomanip>
@@ -22,16 +32,6 @@
 #include <fstream>
 #include <limits>
 
-#include <boost/foreach.hpp>
-#include <boost/tuple/tuple.hpp>
-
-#include <gtsam/3rdparty/Eigen/Eigen/Dense>
-#include <gtsam/3rdparty/Eigen/Eigen/SVD>
-
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/timing.h>
-#include <gtsam/base/Vector.h>
-
 using namespace std;
 
 namespace gtsam {
@@ -684,6 +684,13 @@ Matrix expm(const Matrix& A, size_t K) {
 	return E;
 }
 
+/* ************************************************************************* */
+Matrix Cayley(const Matrix& A) {
+	size_t n = A.cols();
+	assert(A.rows() == n);
+	const Matrix I = eye(n);
+	return (I-A)*inverse(I+A);
+}
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 699bd82ba..51e39ad75 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -22,10 +22,10 @@
 
 #pragma once
 
+#include <gtsam/base/Vector.h>
+#include <gtsam/3rdparty/Eigen/Eigen/QR>
 #include <boost/format.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <gtsam/3rdparty/Eigen/Eigen/QR>
-#include <gtsam/base/Vector.h>
 
 /**
  * Matrix is a typedef in the gtsam namespace
@@ -439,38 +439,40 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
  */
 Matrix expm(const Matrix& A, size_t K=7);
 
+/// Cayley transform
+Matrix Cayley(const Matrix& A);
+
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/split_free.hpp>
 
 namespace boost {
-namespace serialization {
+	namespace serialization {
 
 // split version - sends sizes ahead
-template<class Archive>
-void save(Archive & ar, const gtsam::Matrix & m, unsigned int version)
-{
-	const int rows = m.rows(), cols = m.cols(), elements = rows*cols;
-	std::vector<double> raw_data(elements);
-	std::copy(m.data(), m.data()+elements, raw_data.begin());
-	ar << make_nvp("rows", rows);
-	ar << make_nvp("cols", cols);
-	ar << make_nvp("data", raw_data);
-}
-template<class Archive>
-void load(Archive & ar, gtsam::Matrix & m, unsigned int version)
-{
-	size_t rows, cols;
-	std::vector<double> raw_data;
-	ar >> make_nvp("rows", rows);
-	ar >> make_nvp("cols", cols);
-	ar >> make_nvp("data", raw_data);
-	m = gtsam::Matrix(rows, cols);
-	std::copy(raw_data.begin(), raw_data.end(), m.data());
-}
+		template<class Archive>
+		void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
+			const int rows = m.rows(), cols = m.cols(), elements = rows * cols;
+			std::vector<double> raw_data(elements);
+			std::copy(m.data(), m.data() + elements, raw_data.begin());
+			ar << make_nvp("rows", rows);
+			ar << make_nvp("cols", cols);
+			ar << make_nvp("data", raw_data);
+		}
 
-} // namespace serialization
+		template<class Archive>
+		void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
+			size_t rows, cols;
+			std::vector<double> raw_data;
+			ar >> make_nvp("rows", rows);
+			ar >> make_nvp("cols", cols);
+			ar >> make_nvp("data", raw_data);
+			m = gtsam::Matrix(rows, cols);
+			std::copy(raw_data.begin(), raw_data.end(), m.data());
+		}
+
+	} // namespace serialization
 } // namespace boost
 
 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix)