diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 4ac634f03..c3ab56200 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT( /** * Create a factor graph with projection factors from poses and one calibration * @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 landmarkKey to refer to landmark * @param initialEstimate @@ -97,7 +97,7 @@ std::pair triangulationGraph( /** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) - * @param cameras pinhole cameras + * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param landmarkKey to refer to landmark * @param initialEstimate @@ -106,22 +106,21 @@ std::pair triangulationGraph( template std::pair triangulationGraph( const std::vector& cameras, - const std::vector& measurements, Key landmarkKey, + const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + (camera_i, measurements[i], unit, landmarkKey)); } return std::make_pair(graph, values); } -/// PinholeCamera specific version +/// PinholeCamera specific version // TODO: (chris) why does this exist? template std::pair triangulationGraph( const std::vector >& cameras, @@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector& poses, /** * 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 initialEstimate * @return refined Point3 @@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector& poses, template Point3 triangulateNonlinear( const std::vector& cameras, - const std::vector& measurements, const Point3& initialEstimate) { + const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; @@ -184,7 +183,7 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } -/// PinholeCamera specific version +/// PinholeCamera specific version // TODO: (chris) why does this exist? template Point3 triangulateNonlinear( const std::vector >& cameras,