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

Added tests for printing, plus small formatting

release/4.3a0
Varun Agrawal 2020-12-02 08:04:36 -05:00
parent f7d9710543
commit 916771c02c
6 changed files with 222 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -11,11 +11,12 @@
/**
* @file testCal3Bundler.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Bundler calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Bundler.h>
@ -28,7 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3);
/* ************************************************************************* */
TEST( Cal3Bundler, vector)
TEST(Cal3Bundler, vector)
{
Cal3Bundler K;
Vector expected(3);
@ -37,7 +38,7 @@ TEST( Cal3Bundler, vector)
}
/* ************************************************************************* */
TEST( Cal3Bundler, uncalibrate)
TEST(Cal3Bundler, uncalibrate)
{
Vector v = K.vector() ;
double r = p.x()*p.x() + p.y()*p.y() ;
@ -47,7 +48,7 @@ TEST( Cal3Bundler, uncalibrate)
CHECK(assert_equal(expected,actual));
}
TEST( Cal3Bundler, calibrate )
TEST(Cal3Bundler, calibrate )
{
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -61,7 +62,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
TEST(Cal3Bundler, Duncalibrate)
{
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
@ -74,7 +75,7 @@ TEST( Cal3Bundler, Duncalibrate)
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
TEST(Cal3Bundler, Dcalibrate)
{
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
@ -88,22 +89,32 @@ TEST( Cal3Bundler, Dcalibrate)
}
/* ************************************************************************* */
TEST( Cal3Bundler, assert_equal)
TEST(Cal3Bundler, assert_equal)
{
CHECK(assert_equal(K,K,1e-7));
}
/* ************************************************************************* */
TEST( Cal3Bundler, retract)
TEST(Cal3Bundler, retract)
{
Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
Vector d(3);
Vector3 d;
d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
Cal3Bundler cal(1, 2, 3, 4, 5);
std::stringstream os;
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

14
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -10,12 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3DFisheye.cpp
* @file testCal3Fisheye.cpp
* @brief Unit tests for fisheye calibration class
* @author ghaggin
*/
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h>
@ -198,6 +199,17 @@ TEST(Cal3Fisheye, Dcalibrate)
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Print) {
Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -11,12 +11,13 @@
/**
* @file testCal3DS2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Cal3DS2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3DS2.h>
@ -29,7 +30,7 @@ static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Point2 p(2,3);
/* ************************************************************************* */
TEST( Cal3DS2, uncalibrate)
TEST(Cal3DS2, uncalibrate)
{
Vector k = K.k() ;
double r = p.x()*p.x() + p.y()*p.y() ;
@ -43,7 +44,7 @@ TEST( Cal3DS2, uncalibrate)
CHECK(assert_equal(q,p_i));
}
TEST( Cal3DS2, calibrate )
TEST(Cal3DS2, calibrate )
{
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -54,7 +55,7 @@ TEST( Cal3DS2, calibrate )
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate1)
TEST(Cal3DS2, Duncalibrate1)
{
Matrix computed;
K.uncalibrate(p, computed, boost::none);
@ -65,7 +66,7 @@ TEST( Cal3DS2, Duncalibrate1)
}
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate2)
TEST(Cal3DS2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
@ -79,7 +80,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST( Cal3DS2, Dcalibrate)
TEST(Cal3DS2, Dcalibrate)
{
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -95,7 +96,7 @@ TEST( Cal3DS2, Dcalibrate)
TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST( Cal3DS2, retract)
TEST(Cal3DS2, retract)
{
Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
@ -106,6 +107,17 @@ TEST( Cal3DS2, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
}
/* ************************************************************************* */
TEST(Cal3DS2, Print) {
Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

35
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -10,12 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3Unify.cpp
* @brief Unit tests for transform derivatives
* @file testCal3Unified.cpp
* @brief Unit tests for Cal3Unified calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h>
@ -39,7 +40,7 @@ static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3
static Point2 p(0.5, 0.7);
/* ************************************************************************* */
TEST( Cal3Unified, uncalibrate)
TEST(Cal3Unified, uncalibrate)
{
Point2 p_i(364.7791831734982, 305.6677211952602) ;
Point2 q = K.uncalibrate(p);
@ -47,7 +48,7 @@ TEST( Cal3Unified, uncalibrate)
}
/* ************************************************************************* */
TEST( Cal3Unified, spaceNplane)
TEST(Cal3Unified, spaceNplane)
{
Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
@ -55,7 +56,7 @@ TEST( Cal3Unified, spaceNplane)
}
/* ************************************************************************* */
TEST( Cal3Unified, calibrate)
TEST(Cal3Unified, calibrate)
{
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
@ -65,7 +66,7 @@ TEST( Cal3Unified, calibrate)
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate1)
TEST(Cal3Unified, Duncalibrate1)
{
Matrix computed;
K.uncalibrate(p, computed, boost::none);
@ -74,7 +75,7 @@ TEST( Cal3Unified, Duncalibrate1)
}
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate2)
TEST(Cal3Unified, Duncalibrate2)
{
Matrix computed;
K.uncalibrate(p, boost::none, computed);
@ -87,7 +88,7 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST( Cal3Unified, Dcalibrate)
TEST(Cal3Unified, Dcalibrate)
{
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
@ -99,13 +100,13 @@ TEST( Cal3Unified, Dcalibrate)
}
/* ************************************************************************* */
TEST( Cal3Unified, assert_equal)
TEST(Cal3Unified, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
}
/* ************************************************************************* */
TEST( Cal3Unified, retract)
TEST(Cal3Unified, retract)
{
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
@ -117,7 +118,7 @@ TEST( Cal3Unified, retract)
}
/* ************************************************************************* */
TEST( Cal3Unified, DerivedValue)
TEST(Cal3Unified, DerivedValue)
{
Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
@ -129,6 +130,18 @@ TEST( Cal3Unified, DerivedValue)
CHECK(assert_equal(cal,calafter,1e-9));
}
/* ************************************************************************* */
TEST(Cal3Unified, Print) {
Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
<< ", xi: " << cal.xi();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -11,7 +11,7 @@
/**
* @file testCal3_S2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for basic Cal3_S2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
@ -31,7 +31,7 @@ static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)
TEST(Cal3_S2, easy_constructor)
{
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
@ -43,7 +43,7 @@ TEST( Cal3_S2, easy_constructor)
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate)
TEST(Cal3_S2, calibrate)
{
Point2 intrinsic(2,3);
Point2 expectedimage(1320.3, 1740);
@ -53,7 +53,7 @@ TEST( Cal3_S2, calibrate)
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate_homogeneous) {
TEST(Cal3_S2, calibrate_homogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
@ -61,7 +61,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
TEST(Cal3_S2, Duncalibrate1)
{
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
@ -69,7 +69,7 @@ TEST( Cal3_S2, Duncalibrate1)
}
/* ************************************************************************* */
TEST( Cal3_S2, Duncalibrate2)
TEST(Cal3_S2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
@ -98,7 +98,7 @@ TEST(Cal3_S2, Dcalibrate2)
}
/* ************************************************************************* */
TEST( Cal3_S2, assert_equal)
TEST(Cal3_S2, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
@ -107,7 +107,7 @@ TEST( Cal3_S2, assert_equal)
}
/* ************************************************************************* */
TEST( Cal3_S2, retract)
TEST(Cal3_S2, retract)
{
Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
Vector d(5);
@ -131,8 +131,8 @@ TEST(Cal3_S2, between) {
TEST(Cal3_S2, Print) {
Cal3_S2 cal(5, 5, 5, 5, 5);
std::stringstream os;
os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << " }";
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}

136
gtsam/geometry/tests/testCal3_S2Stereo.cpp Normal file
View File

@ -0,0 +1,136 @@
/* ----------------------------------------------------------------------------
* 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 testCal3_S2Stereo.cpp
* @brief Unit tests for stereo-rig calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
static Point2 p(1, -2);
static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST(Cal3_S2Stereo, easy_constructor) {
Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
double fov = 60; // degrees
size_t w = 640, h = 480;
Cal3_S2Stereo actual(fov, w, h, 3);
CHECK(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, calibrate) {
Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, calibrate_homogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
}
//TODO(Varun) Add calibrate and uncalibrate methods
// /* ************************************************************************* */
// Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
// return k.uncalibrate(pt);
// }
// TEST(Cal3_S2Stereo, Duncalibrate1) {
// Matrix26 computed;
// K.uncalibrate(p, computed, boost::none);
// Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Duncalibrate2) {
// Matrix computed;
// K.uncalibrate(p, boost::none, computed);
// Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
// CHECK(assert_equal(numerical, computed, 1e-9));
// }
// Point2 calibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
// return k.calibrate(pt);
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Dcalibrate1) {
// Matrix computed;
// Point2 expected = K.calibrate(p_uv, computed, boost::none);
// Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
// CHECK(assert_equal(expected, p_xy, 1e-8));
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
// /* ************************************************************************* */
// TEST(Cal3_S2Stereo, Dcalibrate2) {
// Matrix computed;
// Point2 expected = K.calibrate(p_uv, boost::none, computed);
// Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
// CHECK(assert_equal(expected, p_xy, 1e-8));
// CHECK(assert_equal(numerical, computed, 1e-8));
// }
/* ************************************************************************* */
TEST(Cal3_S2Stereo, assert_equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
CHECK(assert_equal(K, K1, 1e-9));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, retract) {
Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
7);
Vector6 d;
d << 1, 2, 3, 4, 5, 6;
Cal3_S2Stereo actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Print) {
Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py()
<< ", b: " << cal.baseline();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */