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

Added triangulation wrapping, tested and works in MATLAB !

release/4.3a0
dellaert 2015-05-03 18:54:17 -07:00
parent 67cf13ad74
commit cd77ec8fd4
2 changed files with 80 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam.h
View File

@ -901,6 +901,16 @@ class StereoCamera {
void serialize() const; void serialize() const;
}; };
#include <gtsam/geometry/triangulation.h>
// Templates appear not yet supported for free functions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
//************************************************************************* //*************************************************************************
// Symbolic // Symbolic
//************************************************************************* //*************************************************************************

70
matlab/gtsam_tests/testTriangulation.m Normal file
View File

@ -0,0 +1,70 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Test triangulation
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Some common constants
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480);
%% Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-pi / 2, 0., -pi / 2);
pose1 = Pose3(upright, Point3(0, 0, 1));
camera1 = SimpleCamera(pose1, sharedCal);
%% create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)));
camera2 = SimpleCamera(pose2, sharedCal);
%% landmark ~5 meters infront of camera
landmark =Point3 (5, 0.5, 1.2);
%% 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark);
z2 = camera2.project(landmark);
%% twoPoses
poses = Pose3Vector;
measurements = Point2Vector;
poses.push_back(pose1);
poses.push_back(pose2);
measurements.push_back(z1);
measurements.push_back(z2);
optimize = true;
rank_tol = 1e-9;
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));
%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector;
measurements.push_back(z1.retract([0.1;0.5]));
measurements.push_back(z2.retract([-0.2;0.3]));
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-2));
%% two Poses with Bundler Calibration
bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480);
camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal);
camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal);
z1 = camera1.project(landmark);
z2 = camera2.project(landmark);
measurements = Point2Vector;
measurements.push_back(z1);
measurements.push_back(z2);
triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize);
CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));