diff --git a/.cproject b/.cproject index 307d0f1cb..f23017ddb 100644 --- a/.cproject +++ b/.cproject @@ -322,6 +322,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -348,7 +356,6 @@ make - tests/testBayesTree.run true false @@ -356,7 +363,6 @@ make - testBinaryBayesNet.run true false @@ -404,7 +410,6 @@ make - testSymbolicBayesNet.run true false @@ -412,7 +417,6 @@ make - tests/testSymbolicFactor.run true false @@ -420,7 +424,6 @@ make - testSymbolicFactorGraph.run true false @@ -436,20 +439,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -484,6 +478,7 @@ make + testGraph.run true false @@ -579,6 +574,7 @@ make + testInference.run true false @@ -586,6 +582,7 @@ make + testGaussianBayesNet.run true false @@ -593,6 +590,7 @@ make + testGaussianFactor.run true false @@ -600,6 +598,7 @@ make + testJunctionTree.run true false @@ -607,6 +606,7 @@ make + testSymbolicBayesNet.run true false @@ -614,6 +614,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +676,14 @@ true true + + make + -j2 + tests/testGeneralSFMFactor.run + true + true + true + make -j2 @@ -957,7 +966,6 @@ make - testErrors.run true false @@ -1317,6 +1325,7 @@ make + testSimulated2DOriented.run true false @@ -1356,6 +1365,7 @@ make + testSimulated2D.run true false @@ -1363,6 +1373,7 @@ make + testSimulated3D.run true false @@ -1450,6 +1461,7 @@ make + tests/testGaussianISAM2 true false @@ -1471,86 +1483,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -1647,6 +1579,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -1679,14 +1699,6 @@ true true - - make - -j2 - check - true - true - true - @@ -2009,6 +2021,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2035,7 +2055,6 @@ make - tests/testBayesTree.run true false @@ -2043,7 +2062,6 @@ make - testBinaryBayesNet.run true false @@ -2091,7 +2109,6 @@ make - testSymbolicBayesNet.run true false @@ -2099,7 +2116,6 @@ make - tests/testSymbolicFactor.run true false @@ -2107,7 +2123,6 @@ make - testSymbolicFactorGraph.run true false @@ -2123,20 +2138,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2171,6 +2177,7 @@ make + testGraph.run true false @@ -2266,6 +2273,7 @@ make + testInference.run true false @@ -2273,6 +2281,7 @@ make + testGaussianBayesNet.run true false @@ -2280,6 +2289,7 @@ make + testGaussianFactor.run true false @@ -2287,6 +2297,7 @@ make + testJunctionTree.run true false @@ -2294,6 +2305,7 @@ make + testSymbolicBayesNet.run true false @@ -2301,6 +2313,7 @@ make + testSymbolicFactorGraph.run true false @@ -2362,6 +2375,14 @@ true true + + make + -j2 + tests/testGeneralSFMFactor.run + true + true + true + make -j2 @@ -2644,7 +2665,6 @@ make - testErrors.run true false @@ -3004,6 +3024,7 @@ make + testSimulated2DOriented.run true false @@ -3043,6 +3064,7 @@ make + testSimulated2D.run true false @@ -3050,6 +3072,7 @@ make + testSimulated3D.run true false @@ -3137,6 +3160,7 @@ make + tests/testGaussianISAM2 true false @@ -3158,86 +3182,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -3334,6 +3278,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -3366,14 +3398,6 @@ true true - - make - -j2 - check - true - true - true - diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index f610f9555..cb790627e 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -17,8 +17,6 @@ */ #pragma once -#ifndef GENERALCAMERAT_H_ -#define GENERALCAMERAT_H_ #include #include @@ -125,9 +123,9 @@ class GeneralCameraT { } GeneralCameraT expmap(const Vector &v) const { - return GeneralCameraT( - calibrated_.expmap(subrange(v,0,Camera::Dim())), - calibration_.expmap(subrange(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()))); + Vector v1 = subrange(v,0,Camera::Dim()); + Vector v2 = subrange(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); + return GeneralCameraT(calibrated_.expmap(v1), calibration_.expmap(v2)); } Vector logmap(const GeneralCameraT &C) const { @@ -221,5 +219,3 @@ typedef GeneralCameraT Cal3DS2Camera; typedef GeneralCameraT Cal3_S2Camera; } - -#endif diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index cd16f6788..4014f1190 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -67,8 +67,9 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const { - Point2 q(camera.project(point,H1,H2) - z_); - return q.vector() ; + Vector error = z_.logmap(camera.project(point,H1,H2)); +// gtsam::print(error, "error"); + return error; } /** return the measured */ diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index ad5ce4a9d..1af7fe401 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -48,7 +48,7 @@ check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLA # Visual SLAM headers += GeneralSFMFactor.h sources += visualSLAM.cpp -check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues +check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues tests/testGeneralSFMFactor # StereoFactor headers += StereoFactor.h diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp new file mode 100644 index 000000000..fb486a8d5 --- /dev/null +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -0,0 +1,172 @@ +/* + * testGeneralSFMFactor.cpp + * + * Created on: Dec 27, 2010 + * Author: nikai + * Description: unit tests for GeneralSFMFactor + */ + +#include +#include + +#include +#include +using namespace boost; + +#define GTSAM_MAGIC_KEY + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Cal3_S2Camera GeneralCamera; +typedef TypedSymbol CameraKey; +typedef TypedSymbol PointKey; +typedef LieValues CameraConfig; +typedef LieValues PointConfig; +typedef TupleValues2 Values; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; + +class Graph: public NonlinearFactorGraph { +public: + void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedGaussian& model) { + push_back(boost::make_shared(z, model, i, j)); + } + + void addCameraConstraint(int j, const GeneralCamera& p) { + boost::shared_ptr factor(new CameraConstraint(j, p)); + push_back(factor); + } +}; + +double getGaussian() +{ + double S,V1,V2; + // Use Box-Muller method to create gauss noise from uniform noise + do + { + double U1 = rand() / (double)(RAND_MAX); + double U2 = rand() / (double)(RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while(S>=1); + return sqrt(-2.0f * (double)log(S) / S) * V1; +} + +typedef NonlinearOptimizer Optimizer; + +// make cube +static Point3 + x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1), + x100(-1, -1, -1), x101(-1, -1, +1), x110(-1, +1, -1), x111(-1, +1, +1); + + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, equals ) +{ + // Create two identical factors and make sure they're equal + Vector z = Vector_(2,323.,240.); + const int cameraFrameNumber=1, landmarkNumber=1; + const SharedGaussian sigma(noiseModel::Unit::Create(1)); + boost::shared_ptr + factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + boost::shared_ptr + factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, error ) { + + Point2 z(3.,0.); + const int cameraFrameNumber=1, landmarkNumber=1; + const SharedGaussian sigma(noiseModel::Unit::Create(1)); + + boost::shared_ptr + factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + // For the following configuration, the factor predicts 320,240 + Values values; + Rot3 R; + Point3 t1(0,0,-6); + Pose3 x1(R,t1); + values.insert(1, GeneralCamera(x1)); + Point3 l1; values.insert(1, l1); + CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, optimize ) { + const SharedGaussian sigma1(noiseModel::Unit::Create(1)); + const double z = 5; + vector L ; + L.push_back(Point3 (-1.0,-1.0, z)); + L.push_back(Point3 (-1.0, 1.0, z)); + L.push_back(Point3 ( 1.0, 1.0, z)); + L.push_back(Point3 ( 1.0,-1.0, z)); + L.push_back(Point3 (-1.0,-1.0, 2*z)); + L.push_back(Point3 (-1.0, 1.0, 2*z)); + L.push_back(Point3 ( 1.0, 1.0, 2*z)); + L.push_back(Point3 ( 1.0,-1.0, 2*z)); + + vector X ; + X.push_back(GeneralCamera(Pose3())); + X.push_back(GeneralCamera(Pose3(eye(3),Point3(0.0,0.0,-z)))); + + // add measurement with noise + shared_ptr graph(new Graph()); + for ( size_t j = 0 ; j < X.size() ; ++j) { + for ( size_t i = 0 ; i < L.size() ; ++i) { + Point2 pt = X[j].project(L[i]) ; + //Point2 q(pt.x()+0.1*getGaussian(), pt.y()+0.1*getGaussian()) ; + graph->addMeasurement(j, i, pt, sigma1); + } + } + + // add initial + const double noise = 1.0; + boost::shared_ptr values(new Values); + for ( int i = 0 ; i < X.size() ; ++i ) + values->insert(i, X[i]) ; + + for ( size_t i = 0 ; i < L.size() ; ++i ) { + Point3 pt(L[i].x()+noise*getGaussian(), + L[i].y()+noise*getGaussian(), + L[i].z()+noise*getGaussian()); + + //if (i == 0) pt = Point3(pt.x()+10, pt.y(), pt.z()) ; + values->insert(i, pt) ; + } + + graph->addCameraConstraint(0, X[0]); + + // Create an ordering of the variables + list keys; + for ( size_t i = 0 ; i < L.size() ; ++i ) keys.push_back(Symbol('l', i)) ; + for ( size_t i = 0 ; i < X.size() ; ++i ) keys.push_back(Symbol('x', i)) ; + shared_ptr ordering(new Ordering(keys)); + + NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-7, 1e-7); + Optimizer optimizer(graph, values, ordering, params); + cout << "before optimization, error is " << optimizer.error() << endl; + Optimizer optimizer2 = optimizer.levenbergMarquardt(); + cout << "after optimization, error is " << optimizer2.error() << endl; + CHECK(optimizer2.error() < 1e-1); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */