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

Switched to TriangulationFactors: huge improvement

release/4.3a0
dellaert 2014-03-02 15:51:02 -05:00
parent 5c466a7914
commit b1013163e7
2 changed files with 10 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam_unstable/geometry/tests/testTriangulation.cpp
View File

@ -16,7 +16,6 @@
* Author: cbeall3 * Author: cbeall3
*/ */
#include <gtsam_unstable/geometry/TriangulationFactor.h>
#include <gtsam_unstable/geometry/triangulation.h> #include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>

23
gtsam_unstable/geometry/triangulation.h
View File

@ -18,10 +18,10 @@
#pragma once #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/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <vector> #include <vector>
@ -56,9 +56,6 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(
const std::vector<Matrix>& projection_matrices, const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol); 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 * 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)); 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];
graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> // PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
(measurements[i], unit2, i, landmarkKey, sharedCal)); graph.push_back(TriangulationFactor<CALIBRATION> //
graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model)); (camera_i, measurements[i], unit2, landmarkKey));
values.insert(i, pose_i);
} }
return std::make_pair(graph, values); 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)); 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 PinholeCamera<CALIBRATION>& camera_i = cameras[i]; const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
boost::shared_ptr<CALIBRATION> // Seems wasteful to create new object graph.push_back(TriangulationFactor<CALIBRATION> //
sharedCal(new CALIBRATION(camera_i.calibration())); (camera_i, measurements[i], unit2, landmarkKey));
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);
} }
return std::make_pair(graph, values); return std::make_pair(graph, values);
} }