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

Moved and refined prototype, trouble with boost::none as argument

release/4.3a0
dellaert 2014-11-27 22:33:54 +01:00
parent 1137cb39d8
commit ec93e0a885
4 changed files with 108 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Cal3_S2.cpp
View File

@ -72,11 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const {
Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal,
FixedRef<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp)
*Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}

66
gtsam/geometry/Cal3_S2.h
View File

@ -26,6 +26,68 @@
namespace gtsam {
/**
* Eigen::Ref like class that cane take either a fixed size or dynamic
* Eigen matrix. In the latter case, the dynamic matrix will be resized.
* Finally, the default constructor acts like boost::none.
*/
template<int Rows, int Cols>
class FixedRef {
public:
/// Fixed size type
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
private:
bool empty_;
Eigen::Map<Fixed> map_;
// Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// to make map_ usurp the memory of the fixed size matrix
void usurp(double* data) {
new (&map_) Eigen::Map<Fixed>(data);
}
public:
/// Defdault constructo acts like boost::none
FixedRef() :
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
FixedRef(Matrix& dynamic) :
empty_(false), map_(NULL) {
}
/// Constructor compatible with old-style
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_;
}
};
/**
* @brief The most common 5DOF 3D->2D calibration
* @addtogroup geometry
@ -148,8 +210,8 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const;
Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(),
FixedRef<2,2> Dp = FixedRef<2,2>()) const;
/**
* convert image coordinates uv to intrinsic coordinates xy

74
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -93,80 +93,6 @@ TEST( Cal3DS2, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
template<int Rows, int Cols>
struct FixedRef {
bool empty_;
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
Eigen::Map<Fixed> map_;
FixedRef() :
empty_(true), map_(NULL) {
}
FixedRef(Fixed& fixed) :
empty_(false), map_(NULL) {
// Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// to make map_ usurp the memory of the fixed size matrix
new (&map_) Eigen::Map<Fixed>(fixed.data());
}
FixedRef(Matrix& dynamic) :
empty_(false), map_(NULL) {
dynamic.resize(Rows, Cols);
// Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// to make map_ usurp the memory of the fixed size matrix
new (&map_) Eigen::Map<Fixed>(dynamic.data());
}
/// Return true is allocated, false if default constructor was used
operator bool() const {
return !empty_;
}
/// Assign a fixed matrix to this
void operator=(const Fixed& fixed) {
map_ << fixed;
}
/// Print
void print(const string& s) {
if (!empty_)
cout << s << map_ << endl;
}
};
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; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

38
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -104,6 +104,44 @@ 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;