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

Comma initialization without .finished()

release/4.3a0
Richard Roberts 2013-10-13 20:21:51 +00:00
parent 0ad2dc6f81
commit 7573b7e582
4 changed files with 285 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

218
gtsam/base/SpecialCommaInitializer.h Normal file
View File

@ -0,0 +1,218 @@
/**
* @file SpecialCommaInitializer.h
* @brief A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix.
* @author Richard Roberts
* @created Oct 10, 2013
*/
#pragma once
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace Eigen {
namespace internal {
// Row-vectors not tested
//template<typename XprType>
//inline void resizeHelper(XprType& xpr, DenseIndex sizeIncrement,
// typename boost::enable_if_c<
// XprType::ColsAtCompileTime == Dynamic && XprType::RowsAtCompileTime == 1>::type* = 0)
//{
// xpr.conservativeResize(xpr.cols() + sizeIncrement);
//}
template<typename XprType>
inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement,
typename boost::enable_if_c<
XprType::RowsAtCompileTime == Dynamic && XprType::ColsAtCompileTime == 1>::type* = 0)
{
xpr.conservativeResize(xpr.rows() + sizeIncrement);
}
template<typename XprType>
inline void resizeHelper(XprType& xpr, typename XprType::Index sizeIncrement,
typename boost::enable_if_c<
XprType::ColsAtCompileTime == Dynamic>::type* = 0)
{
assert(false);
}
}
/// A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix.
template<typename XprType>
class SpecialCommaInitializer :
public CommaInitializer<XprType>,
public MatrixBase<SpecialCommaInitializer<XprType> >
{
private:
bool dynamic_;
public:
typedef MatrixBase<SpecialCommaInitializer<XprType> > Base;
typedef CommaInitializer<XprType> CommaBase;
EIGEN_DENSE_PUBLIC_INTERFACE(SpecialCommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
// Forward to base class
inline SpecialCommaInitializer(XprType& xpr, const typename XprType::Scalar& s, bool dynamic) :
CommaBase(xpr, s), dynamic_(dynamic) {}
// Forward to base class
template<typename OtherDerived>
inline SpecialCommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other, bool dynamic) :
CommaBase(xpr, other), dynamic_(dynamic) {}
inline Index rows() const { return CommaBase::m_xpr.rows(); }
inline Index cols() const { return CommaBase::m_xpr.cols(); }
inline Index outerStride() const { return CommaBase::m_xpr.outerStride(); }
inline Index innerStride() const { return CommaBase::m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const
{
return CommaBase::m_xpr.coeff(row, col);
}
inline CoeffReturnType coeff(Index index) const
{
return CommaBase::m_xpr.coeff(index);
}
inline const Scalar& coeffRef(Index row, Index col) const
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col);
}
inline const Scalar& coeffRef(Index index) const
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(index);
}
inline Scalar& coeffRef(Index row, Index col)
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(Index index)
{
return CommaBase::m_xpr.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return CommaBase::m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
CommaBase::m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return CommaBase::m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
CommaBase::m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
}
const XprType& _expression() const { return CommaBase::m_xpr; }
/// Override base class comma operators to return this class instead of the base class.
SpecialCommaInitializer& operator,(const typename XprType::Scalar& s)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Dynamic);
// Current col should be zero and row should be at the end
assert(CommaBase::m_col == 1);
assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows);
resizeHelper(CommaBase::m_xpr, 1);
}
(void) CommaBase::operator,(s);
return *this;
}
/// Override base class comma operators to return this class instead of the base class.
template<typename OtherDerived>
SpecialCommaInitializer& operator,(const DenseBase<OtherDerived>& other)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Dynamic);
// Current col should be zero and row should be at the end
assert(CommaBase::m_col == 1);
assert(CommaBase::m_row == CommaBase::m_xpr.rows() - CommaBase::m_currentBlockRows);
resizeHelper(CommaBase::m_xpr, other.size());
}
(void) CommaBase::operator,(other);
return *this;
}
};
namespace internal {
template<typename ExpressionType>
struct traits<SpecialCommaInitializer<ExpressionType> > : traits<ExpressionType>
{
};
}
}
namespace gtsam {
class Vec
{
Eigen::VectorXd vector_;
bool dynamic_;
public:
Vec(Eigen::VectorXd::Index size) : vector_(size), dynamic_(false) {}
Vec() : dynamic_(true) {}
Eigen::SpecialCommaInitializer<Eigen::VectorXd> operator<< (double s)
{
if(dynamic_)
vector_.resize(1);
return Eigen::SpecialCommaInitializer<Eigen::VectorXd>(vector_, s, dynamic_);
}
template<typename OtherDerived>
Eigen::SpecialCommaInitializer<Eigen::VectorXd> operator<<(const Eigen::DenseBase<OtherDerived>& other)
{
if(dynamic_)
vector_.resize(other.size());
return Eigen::SpecialCommaInitializer<Eigen::VectorXd>(vector_, other, dynamic_);
}
};
class Mat
{
Eigen::MatrixXd matrix_;
public:
Mat(Eigen::MatrixXd::Index rows, Eigen::MatrixXd::Index cols) : matrix_(rows, cols) {}
Eigen::SpecialCommaInitializer<Eigen::MatrixXd> operator<< (double s)
{
return Eigen::SpecialCommaInitializer<Eigen::MatrixXd>(matrix_, s, false);
}
template<typename OtherDerived>
Eigen::SpecialCommaInitializer<Eigen::MatrixXd> operator<<(const Eigen::DenseBase<OtherDerived>& other)
{
return Eigen::SpecialCommaInitializer<Eigen::MatrixXd>(matrix_, other, false);
}
};
}

1
gtsam/base/Vector.h
View File

@ -25,6 +25,7 @@
#include <iostream> #include <iostream>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/3rdparty/gtsam_eigen_includes.h> #include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam/base/SpecialCommaInitializer.h>
namespace gtsam { namespace gtsam {

45
gtsam/base/tests/testMatrix.cpp
View File

@ -69,6 +69,51 @@ TEST( matrix, Matrix_ )
} }
namespace {
/* ************************************************************************* */
template<typename Derived>
Matrix testFcn1(const Eigen::DenseBase<Derived>& in)
{
return in;
}
/* ************************************************************************* */
template<typename Derived>
Matrix testFcn2(const Eigen::MatrixBase<Derived>& in)
{
return in;
}
}
/* ************************************************************************* */
TEST( matrix, special_comma_initializer)
{
Matrix expected(2,2);
expected(0,0) = 1;
expected(0,1) = 2;
expected(1,0) = 3;
expected(1,1) = 4;
Matrix actual1 = (Mat(2,2) << 1, 2, 3, 4);
Matrix actual2((Mat(2,2) << 1, 2, 3, 4));
Matrix submat1 = (Mat(1,2) << 3, 4);
Matrix actual3 = (Mat(2,2) << 1, 2, submat1);
Matrix submat2 = (Mat(1,2) << 1, 2);
Matrix actual4 = (Mat(2,2) << submat2, 3, 4);
Matrix actual5 = testFcn1((Mat(2,2) << 1, 2, 3, 4));
Matrix actual6 = testFcn2((Mat(2,2) << 1, 2, 3, 4));
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
EXPECT(assert_equal(expected, actual3));
EXPECT(assert_equal(expected, actual4));
EXPECT(assert_equal(expected, actual5));
EXPECT(assert_equal(expected, actual6));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, col_major ) TEST( matrix, col_major )
{ {

127
gtsam/base/tests/testVector.cpp
View File

@ -23,112 +23,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
#include <boost/utility/enable_if.hpp>
// Row-vectors not tested
//template<typename XprType>
//inline void resizeHelper(XprType& xpr, DenseIndex sizeIncrement,
// typename boost::enable_if_c<
// XprType::ColsAtCompileTime == Eigen::Dynamic && XprType::RowsAtCompileTime == 1>::type* = 0)
//{
// xpr.conservativeResize(xpr.cols() + sizeIncrement);
//}
template<typename XprType>
inline void resizeHelper(XprType& xpr, DenseIndex sizeIncrement,
typename boost::enable_if_c<
XprType::RowsAtCompileTime == Eigen::Dynamic && XprType::ColsAtCompileTime == 1>::type* = 0)
{
xpr.conservativeResize(xpr.rows() + sizeIncrement);
}
/// A special comma initializer for Eigen that is implicitly convertible to Vector and Matrix.
template<typename XprType>
class SpecialCommaInitializer : public Eigen::CommaInitializer<XprType>
{
private:
bool dynamic_;
public:
typedef Eigen::CommaInitializer<XprType> Base;
// Forward to base class
inline SpecialCommaInitializer(XprType& xpr, const typename XprType::Scalar& s, bool dynamic) :
Base(xpr, s), dynamic_(dynamic) {}
// Forward to base class
template<typename OtherDerived>
inline SpecialCommaInitializer(XprType& xpr, const Eigen::DenseBase<OtherDerived>& other, bool dynamic) :
Base(xpr, other), dynamic_(dynamic) {}
/// Implicit conversion to expression type, e.g. Vector or Matrix
inline operator XprType ()
{
return this->finished();
}
/// Override base class comma operators to return this class instead of the base class.
SpecialCommaInitializer& operator,(const typename XprType::Scalar& s)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Eigen::Dynamic);
// Current col should be zero and row should be at the end
assert(Base::m_col == 1);
assert(Base::m_row == Base::m_xpr.rows() - Base::m_currentBlockRows);
resizeHelper(Base::m_xpr, 1);
}
(void) Base::operator,(s);
return *this;
}
/// Override base class comma operators to return this class instead of the base class.
template<typename OtherDerived>
SpecialCommaInitializer& operator,(const Eigen::DenseBase<OtherDerived>& other)
{
// If dynamic, resize the underlying object
if(dynamic_)
{
// Dynamic expansion currently only tested for column-vectors
assert(XprType::RowsAtCompileTime == Eigen::Dynamic);
// Current col should be zero and row should be at the end
assert(Base::m_col == 1);
assert(Base::m_row == Base::m_xpr.rows() - Base::m_currentBlockRows);
resizeHelper(Base::m_xpr, other.size());
}
(void) Base::operator,(other);
return *this;
}
};
class Vec
{
Vector vector_;
bool dynamic_;
public:
Vec(DenseIndex size) : vector_(size), dynamic_(false) {}
Vec() : dynamic_(true) {}
SpecialCommaInitializer<Vector> operator<< (double s)
{
if(dynamic_)
vector_.resize(1);
return SpecialCommaInitializer<Vector>(vector_, s, dynamic_);
}
template<typename OtherDerived>
SpecialCommaInitializer<Vector> operator<<(const Eigen::DenseBase<OtherDerived>& other)
{
if(dynamic_)
vector_.resize(other.size());
return SpecialCommaInitializer<Vector>(vector_, other, dynamic_);
}
};
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, Vector_variants ) TEST( TestVector, Vector_variants )
{ {
@ -138,6 +32,22 @@ TEST( TestVector, Vector_variants )
EXPECT(assert_equal(a, b)); EXPECT(assert_equal(a, b));
} }
namespace {
/* ************************************************************************* */
template<typename Derived>
Vector testFcn1(const Eigen::DenseBase<Derived>& in)
{
return in;
}
/* ************************************************************************* */
template<typename Derived>
Vector testFcn2(const Eigen::MatrixBase<Derived>& in)
{
return in;
}
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, special_comma_initializer) TEST( TestVector, special_comma_initializer)
{ {
@ -156,11 +66,16 @@ TEST( TestVector, special_comma_initializer)
Vector subvec2 = (Vec() << 1, 2); Vector subvec2 = (Vec() << 1, 2);
Vector actual5 = (Vec() << subvec2, 3); Vector actual5 = (Vec() << subvec2, 3);
Vector actual6 = testFcn1((Vec() << 1, 2, 3));
Vector actual7 = testFcn2((Vec() << 1, 2, 3));
EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2)); EXPECT(assert_equal(expected, actual2));
EXPECT(assert_equal(expected, actual3)); EXPECT(assert_equal(expected, actual3));
EXPECT(assert_equal(expected, actual4)); EXPECT(assert_equal(expected, actual4));
EXPECT(assert_equal(expected, actual5)); EXPECT(assert_equal(expected, actual5));
EXPECT(assert_equal(expected, actual6));
EXPECT(assert_equal(expected, actual7));
} }
/* ************************************************************************* */ /* ************************************************************************* */