Switched to TriangulationFactors: huge improvement
parent
5c466a7914
commit
b1013163e7
|
@ -16,7 +16,6 @@
|
|||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/TriangulationFactor.h>
|
||||
#include <gtsam_unstable/geometry/triangulation.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
|
@ -18,10 +18,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam_unstable/geometry/TriangulationFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam_unstable/base/dllexport.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
@ -56,9 +56,6 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(
|
|||
const std::vector<Matrix>& projection_matrices,
|
||||
const std::vector<Point2>& measurements, double rank_tol);
|
||||
|
||||
// Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem
|
||||
// We should have a projectionfactor that knows pose is fixed
|
||||
|
||||
///
|
||||
/**
|
||||
* Create a factor graph with projection factors from poses and one calibration
|
||||
|
@ -81,10 +78,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
|||
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
const Pose3& pose_i = poses[i];
|
||||
graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
|
||||
(measurements[i], unit2, i, landmarkKey, sharedCal));
|
||||
graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
|
||||
values.insert(i, pose_i);
|
||||
PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
|
||||
graph.push_back(TriangulationFactor<CALIBRATION> //
|
||||
(camera_i, measurements[i], unit2, landmarkKey));
|
||||
}
|
||||
return std::make_pair(graph, values);
|
||||
}
|
||||
|
@ -110,13 +106,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
|||
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
|
||||
boost::shared_ptr<CALIBRATION> // Seems wasteful to create new object
|
||||
sharedCal(new CALIBRATION(camera_i.calibration()));
|
||||
graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
|
||||
(measurements[i], unit2, i, landmarkKey, sharedCal));
|
||||
const Pose3& pose_i = camera_i.pose();
|
||||
graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
|
||||
values.insert(i, pose_i);
|
||||
graph.push_back(TriangulationFactor<CALIBRATION> //
|
||||
(camera_i, measurements[i], unit2, landmarkKey));
|
||||
}
|
||||
return std::make_pair(graph, values);
|
||||
}
|
||||
|
@ -251,9 +242,9 @@ Point3 triangulatePoint3(
|
|||
typedef PinholeCamera<CALIBRATION> Camera;
|
||||
std::vector<Matrix> projection_matrices;
|
||||
BOOST_FOREACH(const Camera& camera, cameras)
|
||||
projection_matrices.push_back(
|
||||
camera.calibration().K()
|
||||
* sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
|
||||
projection_matrices.push_back(
|
||||
camera.calibration().K()
|
||||
* sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
|
||||
|
||||
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
|
||||
|
||||
|
|
Loading…
Reference in New Issue