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

Some formatting/cleanup before fixing bug

release/4.3a0
dellaert 2015-06-14 10:56:22 -07:00
parent 4909fef21a
commit 2c99f68ed7
3 changed files with 607 additions and 583 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/GeneralSFMFactor.h
View File

@ -59,8 +59,8 @@ namespace gtsam {
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
@ -247,7 +247,7 @@ namespace gtsam {
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:

134
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
void addMeasurement(int i, int j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
@ -65,35 +66,32 @@ public:
};
static double getGaussian()
{
static double getGaussian() {
double S, V1, V2;
// Use Box-Muller method to create gauss noise from uniform noise
do
{
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;
return sqrt(-2.f * (double) log(S) / S) * V1;
}
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
/* ************************************************************************* */
TEST( GeneralSFMFactor, equals )
{
TEST( GeneralSFMFactor, equals ) {
// Create two identical factors and make sure they're equal
Point2 z(323., 240.);
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor1(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor2(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2));
}
@ -109,54 +107,59 @@ TEST( GeneralSFMFactor, error ) {
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values)));
Point3 l1;
values.insert(L(1), l1);
EXPECT(
assert_equal(((Vector ) Vector2(-3., 0.)),
factor.unwhitenedError(values)));
}
static const double baseline = 5.0 ;
static const double baseline = 5.;
/* ************************************************************************* */
static vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> landmarks;
landmarks.push_back(Point3 (-1.0,-1.0, z));
landmarks.push_back(Point3 (-1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0,-1.0, z));
landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks;
}
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.))));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.))));
return X;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3_S2 K(640,480,0.01,320,240);
const Cal3_S2 K(640, 480, 0.1, 320, 240);
vector<GeneralCamera> X;
X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K));
X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K));
return X;
}
static boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
static boost::shared_ptr<Ordering> getOrdering(
const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
for (size_t i = 0; i < landmarks.size(); ++i)
ordering->push_back(L(i));
for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_defaultK ) {
@ -224,8 +227,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
else {
} else {
values.insert(L(i), landmarks[i]);
}
}
@ -299,18 +301,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
Values values;
for (size_t i = 0; i < cameras.size(); ++i) {
const double
rot_noise = 1e-5,
trans_noise = 1e-3,
focal_noise = 1,
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
skew_noise = 1e-5;
if (i == 0) {
values.insert(X(i), cameras[i]);
}
else {
} else {
Vector delta = (Vector(11) <<
rot_noise, rot_noise, rot_noise, // rotation
Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, focal_noise, // f_x, f_y
skew_noise, // s
@ -371,7 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5;
@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.push_back(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(
RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init;
init.insert(X(0), GeneralCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
// The optimal value between the 2m range factor and 1m prior is 1.5m
Values expected;
expected.insert(X(0), GeneralCamera());
expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.)));
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.push_back(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.push_back(
PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.push_back(
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
Values init;
init.insert(X(0), CalibratedCamera());
init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.)));
Values expected;
expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(X(0),
CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
LevenbergMarquardtParams params;
@ -440,13 +450,15 @@ TEST(GeneralSFMFactor, Linearize) {
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
Point3 l1;
values.insert(L(1), l1);
// Test with Empty Model
{
const SharedNoiseModel model;
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
}
@ -454,7 +466,8 @@ TEST(GeneralSFMFactor, Linearize) {
{
const SharedNoiseModel model(noiseModel::Unit::Create(2));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
}
@ -462,7 +475,8 @@ TEST(GeneralSFMFactor, Linearize) {
{
const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
}
@ -470,12 +484,16 @@ TEST(GeneralSFMFactor, Linearize) {
{
const SharedNoiseModel model(noiseModel::Constrained::All(2));
Projection factor(z, model, X(1), L(1));
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(
values);
GaussianFactor::shared_ptr actual = factor.linearize(values);
EXPECT(assert_equal(*expected, *actual, 1e-9));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

94
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -49,7 +49,8 @@ typedef NonlinearEquality<Point3> Point3Constraint;
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
void addMeasurement(const int& i, const int& j, const Point2& z,
const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, X(i), L(j)));
}
@ -65,35 +66,32 @@ public:
};
static double getGaussian()
{
static double getGaussian() {
double S, V1, V2;
// Use Box-Muller method to create gauss noise from uniform noise
do
{
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;
return sqrt(-2.f * (double) log(S) / S) * V1;
}
static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));
/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, equals )
{
TEST( GeneralSFMFactor_Cal3Bundler, equals ) {
// Create two identical factors and make sure they're equal
Point2 z(323., 240.);
const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor1(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor2(
new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
EXPECT(assert_equal(*factor1, *factor2));
}
@ -102,59 +100,66 @@ TEST( GeneralSFMFactor_Cal3Bundler, equals )
TEST( GeneralSFMFactor_Cal3Bundler, error ) {
Point2 z(3., 0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, X(1), L(1)));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, X(1), L(1)));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
Point3 t1(0, 0, -6);
Pose3 x1(R, t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values)));
Point3 l1;
values.insert(L(1), l1);
EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;
static const double baseline = 5.;
/* ************************************************************************* */
static vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> landmarks;
landmarks.push_back(Point3 (-1.0,-1.0, z));
landmarks.push_back(Point3 (-1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0, 1.0, z));
landmarks.push_back(Point3 ( 1.0,-1.0, z));
landmarks.push_back(Point3(-1., -1., z));
landmarks.push_back(Point3(-1., 1., z));
landmarks.push_back(Point3(1., 1., z));
landmarks.push_back(Point3(1., -1., z));
landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, 1.5, 1.5 * z));
landmarks.push_back(Point3(1.5, -1.5, 1.5 * z));
landmarks.push_back(Point3 (-2.0,-2.0, 2*z));
landmarks.push_back(Point3 (-2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0, 2.0, 2*z));
landmarks.push_back(Point3 ( 2.0,-2.0, 2*z));
landmarks.push_back(Point3(-2., -2., 2 * z));
landmarks.push_back(Point3(-2., 2., 2 * z));
landmarks.push_back(Point3(2., 2., 2 * z));
landmarks.push_back(Point3(2., -2., 2 * z));
return landmarks;
}
static vector<GeneralCamera> genCameraDefaultCalibration() {
vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0))));
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0))));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.))));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.))));
return cameras;
}
static vector<GeneralCamera> genCameraVariableCalibration() {
const Cal3Bundler K(500, 1e-3, 1e-3);
vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K));
cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
cameras.push_back(
GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
return cameras;
}
static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
static boost::shared_ptr<Ordering> getOrdering(
const std::vector<GeneralCamera>& cameras,
const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
for (size_t i = 0; i < landmarks.size(); ++i)
ordering->push_back(L(i));
for (size_t i = 0; i < cameras.size(); ++i)
ordering->push_back(X(i));
return ordering;
}
@ -225,8 +230,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
landmarks[i].y() + noise * getGaussian(),
landmarks[i].z() + noise * getGaussian());
values.insert(L(i), pt);
}
else {
} else {
values.insert(L(i), landmarks[i]);
}
}
@ -300,16 +304,13 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
Values values;
for (size_t i = 0; i < cameras.size(); ++i) {
const double
rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3;
const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1,
distort_noise = 1e-3;
if (i == 0) {
values.insert(X(i), cameras[i]);
}
else {
} else {
Vector delta = (Vector(9) <<
rot_noise, rot_noise, rot_noise, // rotation
Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
).finished();
@ -368,7 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
const double reproj_error = 1e-5;
@ -379,5 +382,8 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */