made more generic, and comments
parent
5e660198b6
commit
02c7b2b81d
|
@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
|
||||||
/**
|
/**
|
||||||
* Create a factor graph with projection factors from poses and one calibration
|
* Create a factor graph with projection factors from poses and one calibration
|
||||||
* @param poses Camera poses
|
* @param poses Camera poses
|
||||||
* @param sharedCal shared pointer to single calibration object
|
* @param sharedCal shared pointer to single calibration object (monocular only!)
|
||||||
* @param measurements 2D measurements
|
* @param measurements 2D measurements
|
||||||
* @param landmarkKey to refer to landmark
|
* @param landmarkKey to refer to landmark
|
||||||
* @param initialEstimate
|
* @param initialEstimate
|
||||||
|
@ -97,7 +97,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
||||||
/**
|
/**
|
||||||
* Create a factor graph with projection factors from pinhole cameras
|
* Create a factor graph with projection factors from pinhole cameras
|
||||||
* (each camera has a pose and calibration)
|
* (each camera has a pose and calibration)
|
||||||
* @param cameras pinhole cameras
|
* @param cameras pinhole cameras (monocular or stereo)
|
||||||
* @param measurements 2D measurements
|
* @param measurements 2D measurements
|
||||||
* @param landmarkKey to refer to landmark
|
* @param landmarkKey to refer to landmark
|
||||||
* @param initialEstimate
|
* @param initialEstimate
|
||||||
|
@ -106,22 +106,21 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
||||||
template<class CAMERA>
|
template<class CAMERA>
|
||||||
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
||||||
const std::vector<CAMERA>& cameras,
|
const std::vector<CAMERA>& cameras,
|
||||||
const std::vector<Point2>& measurements, Key landmarkKey,
|
const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
|
||||||
const Point3& initialEstimate) {
|
const Point3& initialEstimate) {
|
||||||
Values values;
|
Values values;
|
||||||
values.insert(landmarkKey, initialEstimate); // Initial landmark value
|
values.insert(landmarkKey, initialEstimate); // Initial landmark value
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
|
static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
|
||||||
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 CAMERA& camera_i = cameras[i];
|
const CAMERA& camera_i = cameras[i];
|
||||||
graph.push_back(TriangulationFactor<CAMERA> //
|
graph.push_back(TriangulationFactor<CAMERA> //
|
||||||
(camera_i, measurements[i], unit2, landmarkKey));
|
(camera_i, measurements[i], unit, landmarkKey));
|
||||||
}
|
}
|
||||||
return std::make_pair(graph, values);
|
return std::make_pair(graph, values);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// PinholeCamera specific version
|
/// PinholeCamera specific version // TODO: (chris) why does this exist?
|
||||||
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<PinholeCamera<CALIBRATION> >& cameras,
|
||||||
|
@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Given an initial estimate , refine a point using measurements in several cameras
|
* Given an initial estimate , refine a point using measurements in several cameras
|
||||||
* @param cameras pinhole cameras
|
* @param cameras pinhole cameras (monocular or stereo)
|
||||||
* @param measurements 2D measurements
|
* @param measurements 2D measurements
|
||||||
* @param initialEstimate
|
* @param initialEstimate
|
||||||
* @return refined Point3
|
* @return refined Point3
|
||||||
|
@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
|
||||||
template<class CAMERA>
|
template<class CAMERA>
|
||||||
Point3 triangulateNonlinear(
|
Point3 triangulateNonlinear(
|
||||||
const std::vector<CAMERA>& cameras,
|
const std::vector<CAMERA>& cameras,
|
||||||
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
|
const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
|
||||||
|
|
||||||
// Create a factor graph and initial values
|
// Create a factor graph and initial values
|
||||||
Values values;
|
Values values;
|
||||||
|
@ -184,7 +183,7 @@ Point3 triangulateNonlinear(
|
||||||
return optimize(graph, values, Symbol('p', 0));
|
return optimize(graph, values, Symbol('p', 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// PinholeCamera specific version
|
/// PinholeCamera specific version // TODO: (chris) why does this exist?
|
||||||
template<class CALIBRATION>
|
template<class CALIBRATION>
|
||||||
Point3 triangulateNonlinear(
|
Point3 triangulateNonlinear(
|
||||||
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
|
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
|
||||||
|
|
Loading…
Reference in New Issue