More adding of static to avoid naming conflicts in unit tests
parent
dd61e5dd58
commit
c443ccbedd
|
@ -24,8 +24,8 @@ using namespace gtsam;
|
|||
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
|
||||
|
||||
Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
|
||||
Point2 p(1, -2);
|
||||
static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
|
||||
static Point2 p(1, -2);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3_S2, easy_constructor)
|
||||
|
|
|
@ -36,37 +36,20 @@ using namespace gtsam;
|
|||
using namespace gtsam::serializationTestHelpers;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Export all classes derived from Value
|
||||
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
|
||||
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
|
||||
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
|
||||
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
|
||||
BOOST_CLASS_EXPORT(gtsam::Point2)
|
||||
BOOST_CLASS_EXPORT(gtsam::Point3)
|
||||
BOOST_CLASS_EXPORT(gtsam::Pose2)
|
||||
BOOST_CLASS_EXPORT(gtsam::Pose3)
|
||||
BOOST_CLASS_EXPORT(gtsam::Rot2)
|
||||
BOOST_CLASS_EXPORT(gtsam::Rot3)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
|
||||
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
|
||||
BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
|
||||
static Point3 pt3(1.0, 2.0, 3.0);
|
||||
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
static Pose3 pose3(rt3, pt3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 pt3(1.0, 2.0, 3.0);
|
||||
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
Pose3 pose3(rt3, pt3);
|
||||
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
|
||||
static Cal3Bundler cal3(1.0, 2.0, 3.0);
|
||||
static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
|
||||
static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
|
||||
static CalibratedCamera cal5(Pose3(rt3, pt3));
|
||||
|
||||
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
|
||||
Cal3Bundler cal3(1.0, 2.0, 3.0);
|
||||
Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
|
||||
Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
|
||||
CalibratedCamera cal5(Pose3(rt3, pt3));
|
||||
|
||||
PinholeCamera<Cal3_S2> cam1(pose3, cal1);
|
||||
StereoCamera cam2(pose3, cal4ptr);
|
||||
StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||
static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
|
||||
static StereoCamera cam2(pose3, cal4ptr);
|
||||
static StereoPoint2 spt(1.0, 2.0, 3.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, text_geometry) {
|
||||
|
|
|
@ -74,21 +74,21 @@ TEST( StereoCamera, project)
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
Pose3 camera1(Matrix_(3,3,
|
||||
static Pose3 camera1(Matrix_(3,3,
|
||||
1., 0., 0.,
|
||||
0.,-1., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,6.25));
|
||||
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
|
||||
StereoCamera stereoCam(Pose3(), K);
|
||||
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
|
||||
static StereoCamera stereoCam(Pose3(), K);
|
||||
|
||||
// point X Y Z in meters
|
||||
Point3 p(0, 0, 5);
|
||||
static Point3 p(0, 0, 5);
|
||||
|
||||
/* ************************************************************************* */
|
||||
StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
|
||||
static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
|
||||
TEST( StereoCamera, Dproject_stereo_pose)
|
||||
{
|
||||
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
|
||||
|
|
|
@ -34,7 +34,7 @@ typedef ISAM<IndexConditional> SymbolicISAM;
|
|||
/* ************************************************************************* */
|
||||
// Some numbers that should be consistent among all smoother tests
|
||||
|
||||
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||
static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -35,7 +35,7 @@ using namespace gtsam;
|
|||
|
||||
static const Index x2=0, x1=1, x3=2, x4=3;
|
||||
|
||||
GaussianFactorGraph createChain() {
|
||||
static GaussianFactorGraph createChain() {
|
||||
|
||||
typedef GaussianFactorGraph::sharedFactor Factor;
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);
|
||||
|
|
|
@ -47,11 +47,11 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
|
|||
|
||||
/* ************************************************************************* */
|
||||
// example noise models
|
||||
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
|
||||
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
|
||||
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
|
||||
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
|
||||
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
|
||||
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
|
||||
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
|
||||
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
|
||||
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
|
||||
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, noiseModels) {
|
||||
|
|
|
@ -47,13 +47,13 @@ typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
|
|||
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 pt3(1.0, 2.0, 3.0);
|
||||
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
Pose3 pose3(rt3, pt3);
|
||||
static Point3 pt3(1.0, 2.0, 3.0);
|
||||
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
static Pose3 pose3(rt3, pt3);
|
||||
|
||||
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
|
||||
Cal3Bundler cal3(1.0, 2.0, 3.0);
|
||||
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
|
||||
static Cal3Bundler cal3(1.0, 2.0, 3.0);
|
||||
|
||||
TEST (Serialization, TemplatedValues) {
|
||||
Values values;
|
||||
|
|
|
@ -28,18 +28,18 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
Pose3 camera1(Matrix_(3,3,
|
||||
static Pose3 camera1(Matrix_(3,3,
|
||||
1., 0., 0.,
|
||||
0.,-1., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,6.25));
|
||||
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
|
||||
StereoCamera stereoCam(Pose3(), K);
|
||||
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
|
||||
static StereoCamera stereoCam(Pose3(), K);
|
||||
|
||||
// point X Y Z in meters
|
||||
Point3 p(0, 0, 5);
|
||||
static Point3 p(0, 0, 5);
|
||||
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
|
||||
|
||||
// Convenience for named keys
|
||||
|
|
|
@ -41,10 +41,10 @@ using symbol_shorthand::L;
|
|||
/* ************************************************************************* */
|
||||
// Some numbers that should be consistent among all smoother tests
|
||||
|
||||
double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
|
||||
static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
|
||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||
|
||||
const double tol = 1e-4;
|
||||
static const double tol = 1e-4;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( ISAM, iSAM_smoother )
|
||||
|
|
|
@ -52,7 +52,7 @@ using symbol_shorthand::L;
|
|||
C3 x1 : x2
|
||||
C4 x7 : x6
|
||||
*/
|
||||
TEST( GaussianJunctionTree, constructor2 )
|
||||
TEST( GaussianJunctionTreeB, constructor2 )
|
||||
{
|
||||
// create a graph
|
||||
Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
|
||||
|
@ -88,7 +88,7 @@ TEST( GaussianJunctionTree, constructor2 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianJunctionTree, optimizeMultiFrontal )
|
||||
TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
|
||||
{
|
||||
// create a graph
|
||||
GaussianFactorGraph fg;
|
||||
|
@ -108,7 +108,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianJunctionTree, optimizeMultiFrontal2)
|
||||
TEST( GaussianJunctionTreeB, optimizeMultiFrontal2)
|
||||
{
|
||||
// create a graph
|
||||
example::Graph nlfg = createNonlinearFactorGraph();
|
||||
|
@ -126,7 +126,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianJunctionTree, slamlike) {
|
||||
TEST(GaussianJunctionTreeB, slamlike) {
|
||||
Values init;
|
||||
planarSLAM::Graph newfactors;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
@ -188,7 +188,7 @@ TEST(GaussianJunctionTree, slamlike) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianJunctionTree, simpleMarginal) {
|
||||
TEST(GaussianJunctionTreeB, simpleMarginal) {
|
||||
|
||||
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
||||
|
||||
|
|
|
@ -37,7 +37,7 @@ typedef PriorFactor<Pose2> PosePrior;
|
|||
typedef NonlinearEquality<Pose2> PoseNLE;
|
||||
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
|
||||
|
||||
Symbol key('x',1);
|
||||
static Symbol key('x',1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( NonlinearEquality, linearization ) {
|
||||
|
@ -241,8 +241,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
|
||||
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
|
||||
static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
||||
|
@ -504,10 +504,10 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
|
|||
}
|
||||
|
||||
// make a realistic calibration matrix
|
||||
double fov = 60; // degrees
|
||||
size_t w=640,h=480;
|
||||
Cal3_S2 K(fov,w,h);
|
||||
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
|
||||
static double fov = 60; // degrees
|
||||
static size_t w=640,h=480;
|
||||
static Cal3_S2 K(fov,w,h);
|
||||
static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
|
||||
|
||||
// typedefs for visual SLAM example
|
||||
typedef visualSLAM::Graph VGraph;
|
||||
|
|
|
@ -174,10 +174,10 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF
|
|||
BOOST_CLASS_EXPORT(gtsam::Pose3)
|
||||
BOOST_CLASS_EXPORT(gtsam::Point3)
|
||||
|
||||
Point3 pt3(1.0, 2.0, 3.0);
|
||||
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
Pose3 pose3(rt3, pt3);
|
||||
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
static Point3 pt3(1.0, 2.0, 3.0);
|
||||
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
|
||||
static Pose3 pose3(rt3, pt3);
|
||||
static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, visual_system) {
|
||||
|
|
Loading…
Reference in New Issue