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

Switched back to PinholeCamera (though I'm not thrilled)

release/4.3a0
dellaert 2015-02-24 14:23:44 +01:00
parent eb28d0ffa8
commit 301e827454
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/triangulation.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/geometry/PinholePose.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/slam/TriangulationFactor.h> #include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -75,8 +75,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i]; const Pose3& pose_i = poses[i];
PinholeBaseK<CALIBRATION> camera_i(pose_i, sharedCal); PinholeCamera<CALIBRATION> camera_i(pose_i, sharedCal);
graph.push_back(TriangulationFactor<PinholeBaseK<CALIBRATION> > // graph.push_back(TriangulationFactor<PinholeCamera<CALIBRATION> > //
(camera_i, measurements[i], unit2, landmarkKey)); (camera_i, measurements[i], unit2, landmarkKey));
} }
return std::make_pair(graph, values); return std::make_pair(graph, values);
@ -108,13 +108,13 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
return std::make_pair(graph, values); return std::make_pair(graph, values);
} }
/// PinholeBaseK specific version /// PinholeCamera specific version
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeBaseK<CALIBRATION> >& cameras, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey, const std::vector<Point2>& measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3& initialEstimate) {
return triangulationGraph<PinholeBaseK<CALIBRATION> > // return triangulationGraph<PinholeCamera<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate); (cameras, measurements, landmarkKey, initialEstimate);
} }
@ -170,12 +170,12 @@ Point3 triangulateNonlinear(const std::vector<CAMERA>& cameras,
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/// PinholeBaseK specific version /// PinholeCamera specific version
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(
const std::vector<PinholeBaseK<CALIBRATION> >& cameras, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) { const std::vector<Point2>& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeBaseK<CALIBRATION> > // return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
(cameras, measurements, initialEstimate); (cameras, measurements, initialEstimate);
} }
@ -278,10 +278,10 @@ Point3 triangulatePoint3(const std::vector<CAMERA>& cameras,
/// Pinhole-specific version /// Pinhole-specific version
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3( Point3 triangulatePoint3(
const std::vector<PinholeBaseK<CALIBRATION> >& cameras, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
return triangulatePoint3<PinholeBaseK<CALIBRATION> > // return triangulatePoint3<PinholeCamera<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize); (cameras, measurements, rank_tol, optimize);
} }