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

Cayley transform

release/4.3a0
Frank Dellaert 2012-01-06 01:26:52 +00:00
parent e6382c7ec0
commit f063308bbc
2 changed files with 44 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/base/Matrix.cpp
View File

@ -15,6 +15,16 @@
* @author Christian Potthast * @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 <stdarg.h>
#include <string.h> #include <string.h>
#include <iomanip> #include <iomanip>
@ -22,16 +32,6 @@
#include <fstream> #include <fstream>
#include <limits> #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; using namespace std;
namespace gtsam { namespace gtsam {
@ -684,6 +684,13 @@ Matrix expm(const Matrix& A, size_t K) {
return E; 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 } // namespace gtsam

14
gtsam/base/Matrix.h
View File

@ -22,10 +22,10 @@
#pragma once #pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/3rdparty/Eigen/Eigen/QR>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/tuple/tuple.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 * Matrix is a typedef in the gtsam namespace
@ -439,6 +439,9 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
*/ */
Matrix expm(const Matrix& A, size_t K=7); Matrix expm(const Matrix& A, size_t K=7);
/// Cayley transform
Matrix Cayley(const Matrix& A);
} // namespace gtsam } // namespace gtsam
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
@ -449,8 +452,7 @@ namespace serialization {
// split version - sends sizes ahead // split version - sends sizes ahead
template<class Archive> template<class Archive>
void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
{
const int rows = m.rows(), cols = m.cols(), elements = rows * cols; const int rows = m.rows(), cols = m.cols(), elements = rows * cols;
std::vector<double> raw_data(elements); std::vector<double> raw_data(elements);
std::copy(m.data(), m.data() + elements, raw_data.begin()); std::copy(m.data(), m.data() + elements, raw_data.begin());
@ -458,9 +460,9 @@ void save(Archive & ar, const gtsam::Matrix & m, unsigned int version)
ar << make_nvp("cols", cols); ar << make_nvp("cols", cols);
ar << make_nvp("data", raw_data); ar << make_nvp("data", raw_data);
} }
template<class Archive> template<class Archive>
void load(Archive & ar, gtsam::Matrix & m, unsigned int version) void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
{
size_t rows, cols; size_t rows, cols;
std::vector<double> raw_data; std::vector<double> raw_data;
ar >> make_nvp("rows", rows); ar >> make_nvp("rows", rows);