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

MAde it more generic

release/4.3a0
dellaert 2015-02-23 22:32:18 +01:00
parent cd10f9aedc
commit 01dc2c61fa
2 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/triangulation.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/geometry/PinholePose.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>
@ -74,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];
PinholePose<CALIBRATION> camera_i(pose_i, sharedCal); PinholeBaseK<CALIBRATION> camera_i(pose_i, sharedCal);
graph.push_back(TriangulationFactor<PinholePose<CALIBRATION> > // graph.push_back(TriangulationFactor<PinholeBaseK<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);
@ -107,13 +108,13 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
return std::make_pair(graph, values); return std::make_pair(graph, values);
} }
/// PinholeCamera specific version /// PinholeBaseK specific version
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const std::vector<PinholeBaseK<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey, const std::vector<Point2>& measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3& initialEstimate) {
return triangulationGraph<PinholeCamera<CALIBRATION> > // return triangulationGraph<PinholeBaseK<CALIBRATION> > //
(cameras, measurements, landmarkKey, initialEstimate); (cameras, measurements, landmarkKey, initialEstimate);
} }
@ -169,12 +170,12 @@ Point3 triangulateNonlinear(const std::vector<CAMERA>& cameras,
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/// PinholeCamera specific version /// PinholeBaseK specific version
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const std::vector<PinholeBaseK<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) { const std::vector<Point2>& measurements, const Point3& initialEstimate) {
return triangulateNonlinear<PinholeCamera<CALIBRATION> > // return triangulateNonlinear<PinholeBaseK<CALIBRATION> > //
(cameras, measurements, initialEstimate); (cameras, measurements, initialEstimate);
} }
@ -277,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<PinholeCamera<CALIBRATION> >& cameras, const std::vector<PinholeBaseK<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<PinholeCamera<CALIBRATION> > // return triangulatePoint3<PinholeBaseK<CALIBRATION> > //
(cameras, measurements, rank_tol, optimize); (cameras, measurements, rank_tol, optimize);
} }

5
gtsam/slam/TriangulationFactor.h
View File

@ -16,8 +16,7 @@
*/ */
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
@ -27,7 +26,7 @@ namespace gtsam {
* The calibration and pose are assumed known. * The calibration and pose are assumed known.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class CAMERA = PinholeCamera<Cal3_S2> > template<class CAMERA>
class TriangulationFactor: public NoiseModelFactor1<Point3> { class TriangulationFactor: public NoiseModelFactor1<Point3> {
public: public: