/* ************************************************************************* */
TEST( InvDepthFactor, optimize) {
// landmark 5 meters infront of camera
Point3 landmark(5, 0, 1);
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
LieScalar inv_depth(1./4);
gtsam::NonlinearFactorGraph graph;
Values initial;
InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma,
Symbol('x',1), Symbol('l',1), Symbol('d',1), K));
graph.push_back(factor);
graph.add(PoseConstraint(Symbol('x',1),level_pose));
initial.insert(Symbol('x',1), level_pose);
initial.insert(Symbol('l',1), inv_landmark);
initial.insert(Symbol('d',1), inv_depth);
LevenbergMarquardtParams lmParams;
Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
EXPECT(assert_equal(initial, result, 1e-9));
/// Add a second camera
// add a camera 1 meter to the right
Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
SimpleCamera right_camera(right_pose, *K);
InvDepthCamera3<Cal3_S2> right_inv_camera(right_pose, K);
Point3 landmark1(6,0,1);
Point2 right_uv = right_camera.project(landmark1);
InverseDepthFactor::shared_ptr factor1(new InverseDepthFactor(right_uv, sigma,
Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
graph.push_back(factor1);
graph.add(PoseConstraint(Symbol('x',2),right_pose));
initial.insert(Symbol('x',2), right_pose);
// TODO: need to add priors to make this work with
// Values result2 = optimize<NonlinearFactorGraph>(graph, initial,
// NonlinearOptimizationParameters(),MULTIFRONTAL, GN);
Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
Point3 l1_result2 = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
result2.at<LieVector>(Symbol('l',1)),
result2.at<LieScalar>(Symbol('d',1)));
EXPECT(assert_equal(landmark1, l1_result2, 1e-9));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project4) {
// landmark 4m to the left and 1m up from camera
// inv depth landmark xyz at origion
Point3 landmark(1, 4, 2);
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))).finished());
double inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project2) {
// landmark 1m to the left and 1m up from camera
// inv landmark xyz is same as camera xyz, so depth actually doesn't matter
Point3 landmark(1, 1, 2);
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))).finished());
double inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( InvDepthFactor, Project1) {
// landmark 5 meters infront of camera
Point3 landmark(5, 0, 1);
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.).finished());
double inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv,1e-8));
EXPECT(assert_equal(Point2(640,480), actual_uv));
}
using namespace boost::assign;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);