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

Fix the template substitution

release/4.3a0
Fan Jiang 2021-12-01 14:46:20 -05:00
parent 13ad7cd88e
commit d84ae6b0c1
2 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -468,9 +468,7 @@ TEST(triangulation, twoPoses_sphericalCamera) {
double rank_tol = 1e-9; double rank_tol = 1e-9;
// 1. Test linear triangulation via DLT // 1. Test linear triangulation via DLT
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> auto projection_matrices = projectionMatricesFromCameras(cameras);
projection_matrices =
getCameraProjectionMatrices<SphericalCamera>(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
EXPECT(assert_equal(landmark, point, 1e-7)); EXPECT(assert_equal(landmark, point, 1e-7));

15
gtsam/geometry/triangulation.h
View File

@ -202,9 +202,10 @@ Point3 triangulateNonlinear(
} }
template<class CAMERA> template<class CAMERA>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices(const CameraSet<CAMERA>& cameras) { std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for(const CAMERA& camera: cameras){ for (const CAMERA &camera: cameras) {
projection_matrices.push_back(camera.getCameraProjectionMatrix()); projection_matrices.push_back(camera.getCameraProjectionMatrix());
} }
return projection_matrices; return projection_matrices;
@ -212,8 +213,8 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMat
// overload, assuming pinholePose // overload, assuming pinholePose
template<class CALIBRATION> template<class CALIBRATION>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices( std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal) { const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (size_t i = 0; i < poses.size(); i++) { for (size_t i = 0; i < poses.size(); i++) {
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal); PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
@ -245,8 +246,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices = auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
getCameraProjectionMatrices< CALIBRATION >(poses, sharedCal);
// Triangulate linearly // Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -293,8 +293,7 @@ Point3 triangulatePoint3(
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices = auto projection_matrices = projectionMatricesFromCameras(cameras);
getCameraProjectionMatrices<CAMERA>(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization