New OptionalJacobian header/cpp, moved unit test to base
dellaert
parent
8bbcc2f3d1
commit
bd342261e4
|
|
@ -534,79 +534,6 @@ Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
|
|||
|
||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
|
||||
|
||||
/**
|
||||
* FixedRef is an Eigen::Ref like class that can take be constructed using
|
||||
* either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
|
||||
* matrix will be resized. Finally, there is a constructor that takes
|
||||
* boost::none, the default constructor acts like boost::none, and
|
||||
* boost::optional<Matrix&> is also supported for backwards compatibility.
|
||||
*/
|
||||
template<int Rows, int Cols>
|
||||
class FixedRef {
|
||||
|
||||
public:
|
||||
|
||||
/// Fixed size type
|
||||
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
|
||||
|
||||
private:
|
||||
|
||||
bool empty_; ///< flag whether initialized or not
|
||||
Eigen::Map<Fixed> map_; /// View on constructor argument, if given
|
||||
|
||||
// Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
|
||||
// uses "placement new" to make map_ usurp the memory of the fixed size matrix
|
||||
void usurp(double* data) {
|
||||
new (&map_) Eigen::Map<Fixed>(data);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/// Default constructor acts like boost::none
|
||||
FixedRef() :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Default constructor acts like boost::none
|
||||
FixedRef(boost::none_t none) :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor that will usurp data of a fixed-size matrix
|
||||
FixedRef(Fixed& fixed) :
|
||||
empty_(false), map_(NULL) {
|
||||
usurp(fixed.data());
|
||||
}
|
||||
|
||||
/// Constructor that will resize a dynamic matrix (unless already correct)
|
||||
FixedRef(Matrix& dynamic) :
|
||||
empty_(false), map_(NULL) {
|
||||
dynamic.resize(Rows, Cols); // no malloc if correct size
|
||||
usurp(dynamic.data());
|
||||
}
|
||||
|
||||
/// Constructor compatible with old-style derivatives
|
||||
FixedRef(const boost::optional<Matrix&> optional) :
|
||||
empty_(!optional), map_(NULL) {
|
||||
if (optional) {
|
||||
optional->resize(Rows, Cols);
|
||||
usurp(optional->data());
|
||||
}
|
||||
}
|
||||
|
||||
/// Return true is allocated, false if default constructor was used
|
||||
operator bool() const {
|
||||
return !empty_;
|
||||
}
|
||||
|
||||
/// De-reference, like boost optional
|
||||
Eigen::Map<Fixed>& operator* () {
|
||||
return map_;
|
||||
}
|
||||
|
||||
/// TODO: operator->()
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,115 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 OptionalJacobian.h
|
||||
* @brief Special class for optional Matrix arguments
|
||||
* @author Frank Dellaert
|
||||
* @author Natesh Srinivasan
|
||||
* @date Nov 28, 2014
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
|
||||
|
||||
#ifndef OPTIONALJACOBIAN_NOBOOST
|
||||
#include <boost/optional.hpp>
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* OptionalJacobian is an Eigen::Ref like class that can take be constructed using
|
||||
* either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
|
||||
* matrix will be resized. Finally, there is a constructor that takes
|
||||
* boost::none, the default constructor acts like boost::none, and
|
||||
* boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
|
||||
*/
|
||||
template<int Rows, int Cols>
|
||||
class OptionalJacobian {
|
||||
|
||||
public:
|
||||
|
||||
/// Fixed size type
|
||||
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
|
||||
|
||||
private:
|
||||
|
||||
bool empty_; ///< flag whether initialized or not
|
||||
Eigen::Map<Fixed> map_; /// View on constructor argument, if given
|
||||
|
||||
// Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
|
||||
// uses "placement new" to make map_ usurp the memory of the fixed size matrix
|
||||
void usurp(double* data) {
|
||||
new (&map_) Eigen::Map<Fixed>(data);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
/// Default constructor acts like boost::none
|
||||
OptionalJacobian() :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor that will usurp data of a fixed-size matrix
|
||||
OptionalJacobian(Fixed& fixed) :
|
||||
empty_(false), map_(NULL) {
|
||||
usurp(fixed.data());
|
||||
}
|
||||
|
||||
/// Constructor that will usurp data of a fixed-size matrix, pointer version
|
||||
OptionalJacobian(Fixed* fixedPtr) :
|
||||
empty_(fixedPtr==NULL), map_(NULL) {
|
||||
if (fixedPtr)
|
||||
usurp(fixedPtr->data());
|
||||
}
|
||||
|
||||
/// Constructor that will resize a dynamic matrix (unless already correct)
|
||||
OptionalJacobian(Eigen::MatrixXd& dynamic) :
|
||||
empty_(false), map_(NULL) {
|
||||
dynamic.resize(Rows, Cols); // no malloc if correct size
|
||||
usurp(dynamic.data());
|
||||
}
|
||||
|
||||
#ifndef OPTIONALJACOBIAN_NOBOOST
|
||||
|
||||
/// Constructor with boost::none just makes empty
|
||||
OptionalJacobian(boost::none_t none) :
|
||||
empty_(true), map_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor compatible with old-style derivatives
|
||||
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
|
||||
empty_(!optional), map_(NULL) {
|
||||
if (optional) {
|
||||
optional->resize(Rows, Cols);
|
||||
usurp(optional->data());
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/// Return true is allocated, false if default constructor was used
|
||||
operator bool() const {
|
||||
return !empty_;
|
||||
}
|
||||
|
||||
/// De-reference, like boost optional
|
||||
Eigen::Map<Fixed>& operator*() {
|
||||
return map_;
|
||||
}
|
||||
|
||||
/// TODO: operator->()
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testOptionalJacobian.cpp
|
||||
* @brief Unit test for OptionalJacobian
|
||||
* @author Frank Dellaert
|
||||
* @date Nov 28, 2014
|
||||
**/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST( OptionalJacobian, Constructors )
|
||||
{
|
||||
Matrix23 fixed;
|
||||
Matrix dynamic;
|
||||
OptionalJacobian<2,3> H1;
|
||||
OptionalJacobian<2,3> H2(fixed);
|
||||
OptionalJacobian<2,3> H3(&fixed);
|
||||
OptionalJacobian<2,3> H4(dynamic);
|
||||
OptionalJacobian<2,3> H5(boost::none);
|
||||
boost::optional<Matrix&> optional(dynamic);
|
||||
OptionalJacobian<2,3> H6(optional);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) {
|
||||
if (H)
|
||||
*H = Matrix23::Zero();
|
||||
}
|
||||
|
||||
TEST( OptionalJacobian, Ref2) {
|
||||
|
||||
Matrix expected;
|
||||
expected = Matrix23::Zero();
|
||||
|
||||
// Default argument does nothing
|
||||
test3();
|
||||
|
||||
// Fixed size, no copy
|
||||
Matrix23 fixed1;
|
||||
fixed1.setOnes();
|
||||
test3(fixed1);
|
||||
EXPECT(assert_equal(expected,fixed1));
|
||||
|
||||
// Fixed size, no copy, pointer style
|
||||
Matrix23 fixed2;
|
||||
fixed2.setOnes();
|
||||
test3(&fixed2);
|
||||
EXPECT(assert_equal(expected,fixed2));
|
||||
|
||||
// Empty is no longer a sign we don't want a matrix, we want it resized
|
||||
Matrix dynamic0;
|
||||
test3(dynamic0);
|
||||
EXPECT(assert_equal(expected,dynamic0));
|
||||
|
||||
// Dynamic wrong size
|
||||
Matrix dynamic1(3,5);
|
||||
dynamic1.setOnes();
|
||||
test3(dynamic1);
|
||||
EXPECT(assert_equal(expected,dynamic0));
|
||||
|
||||
// Dynamic right size
|
||||
Matrix dynamic2(2,5);
|
||||
dynamic2.setOnes();
|
||||
test3(dynamic2);
|
||||
EXPECT(assert_equal(dynamic2,dynamic0));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -104,44 +104,6 @@ TEST(Cal3_S2, between) {
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void test3(FixedRef<2,3> H = FixedRef<2,3>()) {
|
||||
if (H)
|
||||
*H = Matrix23::Zero();
|
||||
}
|
||||
|
||||
TEST( Cal3DS2, Ref2) {
|
||||
|
||||
Matrix expected;
|
||||
expected = Matrix23::Zero();
|
||||
|
||||
// Default argument does nothing
|
||||
test3();
|
||||
|
||||
// Fixed size, no copy
|
||||
Matrix23 fixedDcal;
|
||||
fixedDcal.setOnes();
|
||||
test3(fixedDcal);
|
||||
EXPECT(assert_equal(expected,fixedDcal));
|
||||
|
||||
// Empty is no longer a sign we don't want a matrix, we want it resized
|
||||
Matrix dynamic0;
|
||||
test3(dynamic0);
|
||||
EXPECT(assert_equal(expected,dynamic0));
|
||||
|
||||
// Dynamic wrong size
|
||||
Matrix dynamic1(3,5);
|
||||
dynamic1.setOnes();
|
||||
test3(dynamic1);
|
||||
EXPECT(assert_equal(expected,dynamic0));
|
||||
|
||||
// Dynamic right size
|
||||
Matrix dynamic2(2,5);
|
||||
dynamic2.setOnes();
|
||||
test3(dynamic2);
|
||||
EXPECT(assert_equal(dynamic2,dynamic0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue