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

EPI: Optimize landmark position using LM using SVD triangulation as initialization. 

Works for both single camera and multi-camera (with different calibration) cases.
release/4.3a0
Zsolt Kira 2013-10-18 02:56:10 +00:00
parent c547a456e9
commit 9997e0b8ea
2 changed files with 137 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
gtsam_unstable/geometry/triangulation.cpp
View File

@ -21,15 +21,28 @@
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/ProjectionFactor.h>
using namespace std;
using namespace boost::assign;
namespace gtsam {
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> ProjectionFactor;
typedef PriorFactor<Pose3> Pose3Prior;
/* ************************************************************************* */
// See Hartley and Zisserman, 2nd Ed., page 312
Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
const vector<Point2>& measurements, double rank_tol) {
Point3 triangulateDLT(const std::vector<Pose3>& poses, const vector<Matrix>& projection_matrices,
const vector<Point2>& measurements, const Cal3_S2 &K, double rank_tol, bool optimize) {
Matrix A = zeros(projection_matrices.size() *2, 4);
@ -51,7 +64,117 @@ Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
if(rank < 3)
throw(TriangulationUnderconstrainedException());
return Point3(sub( (v / v(3)),0,3));
Point3 point = Point3(sub( (v / v(3)),0,3));
if (optimize) {
NonlinearFactorGraph graph;
gtsam::Values::shared_ptr values(new gtsam::Values());
static SharedNoiseModel noise(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6)));
int ij = 0;
BOOST_FOREACH(const Point2 &measurement, measurements) {
// Factor for pose i
ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), boost::make_shared<Cal3_S2>(K));
graph.push_back( boost::make_shared<ProjectionFactor>(*projectionFactor) );
// Prior on pose
graph.push_back(Pose3Prior(X(ij), poses[ij], prior_model));
// Initial pose values
values->insert( X(ij), poses[ij]);
ij++;
}
// Initial landmark value
values->insert(L(0), point);
// Optimize
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.lambdaInitial = 1;
params.lambdaFactor = 10;
params.maxIterations = 100;
params.absoluteErrorTol = 1.0;
params.verbosityLM = LevenbergMarquardtParams::SILENT;
params.verbosity = NonlinearOptimizerParams::SILENT;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
LevenbergMarquardtOptimizer optimizer(graph, *values, params);
Values result = optimizer.optimize();
point = result.at<Point3>(L(0));
}
return point;
}
/* ************************************************************************* */
// See Hartley and Zisserman, 2nd Ed., page 312
Point3 triangulateDLT(const std::vector<Pose3>& poses, const vector<Matrix>& projection_matrices,
const vector<Point2>& measurements, const vector<Cal3_S2::shared_ptr> &Ks, double rank_tol, bool optimize) {
Matrix A = zeros(projection_matrices.size() *2, 4);
for(size_t i=0; i< projection_matrices.size(); i++) {
size_t row = i*2;
const Matrix& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i);
// build system of equations
A.row(row) = p.x() * projection.row(2) - projection.row(0);
A.row(row+1) = p.y() * projection.row(2) - projection.row(1);
}
int rank;
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol);
// std::cout << "s " << s.transpose() << std:endl;
if(rank < 3)
throw(TriangulationUnderconstrainedException());
Point3 point = Point3(sub( (v / v(3)),0,3));
if (optimize) {
NonlinearFactorGraph graph;
gtsam::Values::shared_ptr values(new gtsam::Values());
static SharedNoiseModel noise(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6)));
int ij = 0;
BOOST_FOREACH(const Point2 &measurement, measurements) {
// Factor for pose i
ProjectionFactor *projectionFactor = new ProjectionFactor(measurement, noise, X(ij), L(0), Ks[ij]);
graph.push_back( boost::make_shared<ProjectionFactor>(*projectionFactor) );
// Prior on pose
graph.push_back(Pose3Prior(X(ij), poses[ij], prior_model));
// Initial pose values
values->insert( X(ij), poses[ij]);
ij++;
}
// Initial landmark value
values->insert(L(0), point);
// Optimize
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.lambdaInitial = 1;
params.lambdaFactor = 10;
params.maxIterations = 100;
params.absoluteErrorTol = 1.0;
params.verbosityLM = LevenbergMarquardtParams::SILENT;
params.verbosity = NonlinearOptimizerParams::SILENT;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
LevenbergMarquardtOptimizer optimizer(graph, *values, params);
Values result = optimizer.optimize();
point = result.at<Point3>(L(0));
}
return point;
}
/* ************************************************************************* */

17
gtsam_unstable/geometry/triangulation.h
View File

@ -47,8 +47,12 @@ public:
}
};
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, const Cal3_S2 &K, double rank_tol, bool optimize);
Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, const std::vector<Cal3_S2::shared_ptr> &Ks, double rank_tol, bool optimize);
/**
* Function to triangulate 3D landmark point from an arbitrary number
@ -63,7 +67,7 @@ Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
*/
template<class CALIBRATION>
GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const CALIBRATION& K, double rank_tol = 1e-9){
const std::vector<Point2>& measurements, const CALIBRATION& K, double rank_tol = 1e-9, bool optimize = false){
assert(poses.size() == measurements.size());
@ -79,7 +83,7 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// std::cout << "rank_tol i \n" << rank_tol << std::endl;
}
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, K, rank_tol, optimize);
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
@ -94,7 +98,8 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
/* ************************************************************************* */
template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol) {
const std::vector<Point2>& measurements, const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
bool optimize = false) {
assert(poses.size() == measurements.size());
assert(poses.size() == Ks.size());
@ -111,7 +116,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// std::cout << "2rank_tol i \n" << rank_tol << std::endl;
}
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, Ks, rank_tol, optimize);
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {