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

convenience function to access right image Point2 and trivial unit tests

release/4.3a0
Chris Beall 2013-10-18 19:07:42 +00:00
parent 40bb5ba079
commit 030d773b6d
2 changed files with 50 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/StereoPoint2.h
View File

@ -147,6 +147,11 @@ namespace gtsam {
return Point2(uL_, v_);
}
/** convenient function to get a Point2 from the right image */
inline Point2 right(){
return Point2(uR_, v_);
}
private:
/// @}

51
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -11,26 +11,65 @@
/**
* @file testStereoPoint2.cpp
*
* @brief Tests for the StereoPoint2 class
*
* @date Nov 4, 2011
* @author Alex Cunningham
*/
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/lieProxies.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/StereoPoint2.h>
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2)
/* ************************************************************************* */
TEST(testStereoPoint2, test) {
/* ************************************************************************* */
TEST(StereoPoint2, constructor) {
StereoPoint2 p1(1, 2, 3), p2 = p1;
EXPECT(assert_equal(p1, p2));
}
/* ************************************************************************* */
TEST(StereoPoint2, Lie) {
StereoPoint2 p1(1, 2, 3), p2(4, 5, 6);
Matrix H1, H2;
EXPECT(assert_equal(StereoPoint2(5,7,9), p1.compose(p2)));
EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2)));
EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract(Vector_(3, 4.,5.,6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */
TEST( StereoPoint2, expmap) {
Vector d(3);
d(0) = 1;
d(1) = -1;
d(2) = -3;
StereoPoint2 a(4, 5, 6), b = a.retract(d), c(5, 4, 3);
EXPECT(assert_equal(b,c));
}
/* ************************************************************************* */
TEST( StereoPoint2, arithmetic) {
EXPECT(assert_equal( StereoPoint2(5,6,7), StereoPoint2(4,5,6)+StereoPoint2(1,1,1)));
EXPECT(assert_equal( StereoPoint2(3,4,5), StereoPoint2(4,5,6)-StereoPoint2(1,1,1)));
}
/* ************************************************************************* */
TEST(testStereoPoint2, left_right) {
StereoPoint2 p1(1,2,3);
EXPECT(assert_equal( Point2(1,3), p1.point2()));
EXPECT(assert_equal( Point2(2,3), p1.right()));
}
/* ************************************************************************* */