testGeneralSFMFactor_Cal3Bundler.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/base/Testable.h>

#include <memory>
#include <CppUnitLite/TestHarness.h>

#include <iostream>
#include <vector>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef NonlinearEquality<Point3> Point3Constraint;

/* ************************************************************************* */
class Graph: public NonlinearFactorGraph {
public:
  void addMeasurement(const int& i, const int& j, const Point2& z,
      const SharedNoiseModel& model) {
    emplace_shared<Projection>(z, model, X(i), L(j));
  }

  void addCameraConstraint(int j, const GeneralCamera& p) {
    std::shared_ptr<CameraConstraint> factor(new CameraConstraint(X(j), p));
    push_back(factor);
  }

  void addPoint3Constraint(int j, const Point3& p) {
    std::shared_ptr<Point3Constraint> factor(new Point3Constraint(L(j), p));
    push_back(factor);
  }

};

static 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.f * (double) log(S) / S) * V1;
}

static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2));

/* ************************************************************************* */
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));
  std::shared_ptr<Projection> factor1(
      new Projection(z, sigma, cameraFrameNumber, landmarkNumber));

  std::shared_ptr<Projection> factor2(
      new Projection(z, sigma, cameraFrameNumber, landmarkNumber));

  EXPECT(assert_equal(*factor1, *factor2));
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, error ) {
  Point2 z(3., 0.);
  const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
  std::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(0,0,0);
  values.insert(L(1), l1);
  EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values)));
}

static const double baseline = 5.;

/* ************************************************************************* */
static vector<Point3> genPoint3() {
  const double z = 5;
  vector<Point3> landmarks;
  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., -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(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(Rot3(), Point3(-baseline / 2., 0., 0.)), K));
  cameras.push_back(
      GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K));
  return cameras;
}

static std::shared_ptr<Ordering> getOrdering(
    const std::vector<GeneralCamera>& cameras,
    const std::vector<Point3>& landmarks) {
  std::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));
  return ordering;
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraDefaultCalibration();

  // add measurement with noise
  Graph graph;
  for (size_t j = 0; j < cameras.size(); ++j) {
    for (size_t i = 0; i < landmarks.size(); ++i) {
      Point2 pt = cameras[j].project(landmarks[i]);
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size() * landmarks.size();

  // add initial
  const double noise = baseline * 0.1;
  Values values;
  for (size_t i = 0; i < cameras.size(); ++i)
    values.insert(X(i), cameras[i]);

  for (size_t i = 0; i < landmarks.size(); ++i) {
    Point3 pt(landmarks[i].x() + noise * getGaussian(),
        landmarks[i].y() + noise * getGaussian(),
        landmarks[i].z() + noise * getGaussian());
    values.insert(L(i), pt);
  }

  graph.addCameraConstraint(0, cameras[0]);

  // Create an ordering of the variables
  Ordering ordering = *getOrdering(cameras, landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();
  // add measurement with noise
  Graph graph;
  for (size_t j = 0; j < cameras.size(); ++j) {
    for (size_t i = 0; i < landmarks.size(); ++i) {
      Point2 pt = cameras[j].project(landmarks[i]);
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size() * landmarks.size();

  // add initial
  const double noise = baseline * 0.1;
  Values values;
  for (size_t i = 0; i < cameras.size(); ++i)
    values.insert(X(i), cameras[i]);

  // add noise only to the first landmark
  for (size_t i = 0; i < landmarks.size(); ++i) {
    if (i == 0) {
      Point3 pt(landmarks[i].x() + noise * getGaussian(),
          landmarks[i].y() + noise * getGaussian(),
          landmarks[i].z() + noise * getGaussian());
      values.insert(L(i), pt);
    } else {
      values.insert(L(i), landmarks[i]);
    }
  }

  graph.addCameraConstraint(0, cameras[0]);
  const double reproj_error = 1e-5;

  Ordering ordering = *getOrdering(cameras, landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  const double noise = baseline * 0.1;
  Graph graph;
  for (size_t j = 0; j < cameras.size(); ++j) {
    for (size_t i = 0; i < landmarks.size(); ++i) {
      Point2 pt = cameras[j].project(landmarks[i]);
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = landmarks.size() * cameras.size();

  Values values;
  for (size_t i = 0; i < cameras.size(); ++i)
    values.insert(X(i), cameras[i]);

  for (size_t i = 0; i < landmarks.size(); ++i) {
    Point3 pt(landmarks[i].x() + noise * getGaussian(),
        landmarks[i].y() + noise * getGaussian(),
        landmarks[i].z() + noise * getGaussian());
    //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z());
    values.insert(L(i), pt);
  }

  for (size_t i = 0; i < cameras.size(); ++i)
    graph.addCameraConstraint(i, cameras[i]);

  const double reproj_error = 1e-5;

  Ordering ordering = *getOrdering(cameras, landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {

  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  Graph graph;
  for (size_t j = 0; j < cameras.size(); ++j) {
    for (size_t i = 0; i < landmarks.size(); ++i) {
      Point2 pt = cameras[j].project(landmarks[i]);
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = landmarks.size() * cameras.size();

  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;
    if (i == 0) {
      values.insert(X(i), cameras[i]);
    } else {

      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();
      values.insert(X(i), cameras[i].retract(delta));
    }
  }

  for (size_t i = 0; i < landmarks.size(); ++i) {
    values.insert(L(i), landmarks[i]);
  }

  // fix X0 and all landmarks, allow only the cameras[1] to move
  graph.addCameraConstraint(0, cameras[0]);
  for (size_t i = 0; i < landmarks.size(); ++i)
    graph.addPoint3Constraint(i, landmarks[i]);

  const double reproj_error = 1e-5;

  Ordering ordering = *getOrdering(cameras, landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
  vector<Point3> landmarks = genPoint3();
  vector<GeneralCamera> cameras = genCameraVariableCalibration();

  // add measurement with noise
  Graph graph;
  for (size_t j = 0; j < cameras.size(); ++j) {
    for (size_t i = 0; i < landmarks.size(); ++i) {
      Point2 pt = cameras[j].project(landmarks[i]);
      graph.addMeasurement(j, i, pt, sigma1);
    }
  }

  const size_t nMeasurements = cameras.size() * landmarks.size();

  // add initial
  const double noise = baseline * 0.1;
  Values values;
  for (size_t i = 0; i < cameras.size(); ++i)
    values.insert(X(i), cameras[i]);

  // add noise only to the first landmark
  for (size_t i = 0; i < landmarks.size(); ++i) {
    Point3 pt(landmarks[i].x() + noise * getGaussian(),
        landmarks[i].y() + noise * getGaussian(),
        landmarks[i].z() + noise * getGaussian());
    values.insert(L(i), pt);
  }

  // Constrain position of system with the first camera constrained to the origin
  graph.addCameraConstraint(0, cameras[0]);

  // Constrain the scale of the problem with a soft range factor of 1m between the cameras
  graph.emplace_shared<
      RangeFactor<GeneralCamera, GeneralCamera> >(X(0), X(1), 2.,
          noiseModel::Isotropic::Sigma(1, 10.));

  const double reproj_error = 1e-5;

  Ordering ordering = *getOrdering(cameras, landmarks);
  LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
  Values final = optimizer.optimize();
  EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */