Merge branch 'feature/LevenbergMarquardt'
Luca
commit
727679ea1d
|
|
@ -224,10 +224,10 @@ namespace gtsam {
|
|||
std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
|
||||
|
||||
/** Return only the diagonal of the Hessian A'*A, as a VectorValues */
|
||||
VectorValues hessianDiagonal() const;
|
||||
virtual VectorValues hessianDiagonal() const;
|
||||
|
||||
/** Return the block diagonal of the Hessian for this factor */
|
||||
std::map<Key,Matrix> hessianBlockDiagonal() const;
|
||||
virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
|
||||
|
||||
/** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
|
||||
* the dense elimination function specified in \c function (default EliminatePreferCholesky),
|
||||
|
|
|
|||
|
|
@ -11,9 +11,10 @@
|
|||
|
||||
/**
|
||||
* @file testGaussianFactorGraphUnordered.cpp
|
||||
* @brief Unit tests for Linear Factor
|
||||
* @brief Unit tests for Linear Factor Graph
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Richard Roberts
|
||||
**/
|
||||
|
||||
|
|
@ -325,10 +326,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd3 )
|
|||
EXPECT(assert_equal(Y,AtA*X));
|
||||
|
||||
double* x = &X[0];
|
||||
double* y = &Y[0];
|
||||
|
||||
Vector fast_y = gtsam::zero(6);
|
||||
double* actual = &fast_y[0];
|
||||
gfg.multiplyHessianAdd(1.0, x, fast_y.data());
|
||||
EXPECT(assert_equal(Y, fast_y));
|
||||
|
||||
|
|
@ -409,6 +408,21 @@ TEST( GaussianFactorGraph, negate ) {
|
|||
EXPECT(assert_equal(expNegation, actNegation));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianFactorGraph, hessianDiagonal )
|
||||
{
|
||||
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
|
||||
VectorValues expected;
|
||||
Matrix infoMatrix = gfg.hessian().first;
|
||||
Vector d = infoMatrix.diagonal();
|
||||
|
||||
VectorValues actual = gfg.hessianDiagonal();
|
||||
expected.insert(0, d.segment<2>(0));
|
||||
expected.insert(1, d.segment<2>(2));
|
||||
expected.insert(2, d.segment<2>(4));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -31,37 +31,61 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using boost::adaptors::map_values;
|
||||
|
||||
/* ************************************************************************* */
|
||||
LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const {
|
||||
std::string s = src; boost::algorithm::to_upper(s);
|
||||
if (s == "SILENT") return LevenbergMarquardtParams::SILENT;
|
||||
if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA;
|
||||
if (s == "TRYLAMBDA") return LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (s == "TRYCONFIG") return LevenbergMarquardtParams::TRYCONFIG;
|
||||
if (s == "TRYDELTA") return LevenbergMarquardtParams::TRYDELTA;
|
||||
if (s == "DAMPED") return LevenbergMarquardtParams::DAMPED;
|
||||
LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(
|
||||
const std::string &src) const {
|
||||
std::string s = src;
|
||||
boost::algorithm::to_upper(s);
|
||||
if (s == "SILENT")
|
||||
return LevenbergMarquardtParams::SILENT;
|
||||
if (s == "LAMBDA")
|
||||
return LevenbergMarquardtParams::LAMBDA;
|
||||
if (s == "TRYLAMBDA")
|
||||
return LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (s == "TRYCONFIG")
|
||||
return LevenbergMarquardtParams::TRYCONFIG;
|
||||
if (s == "TRYDELTA")
|
||||
return LevenbergMarquardtParams::TRYDELTA;
|
||||
if (s == "DAMPED")
|
||||
return LevenbergMarquardtParams::DAMPED;
|
||||
|
||||
/* default is silent */
|
||||
return LevenbergMarquardtParams::SILENT;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) const {
|
||||
std::string LevenbergMarquardtParams::verbosityLMTranslator(
|
||||
VerbosityLM value) const {
|
||||
std::string s;
|
||||
switch (value) {
|
||||
case LevenbergMarquardtParams::SILENT: s = "SILENT" ; break;
|
||||
case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION" ; break;
|
||||
case LevenbergMarquardtParams::LAMBDA: s = "LAMBDA" ; break;
|
||||
case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ; break;
|
||||
case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ; break;
|
||||
case LevenbergMarquardtParams::TRYDELTA: s = "TRYDELTA" ; break;
|
||||
case LevenbergMarquardtParams::DAMPED: s = "DAMPED" ; break;
|
||||
default: s = "UNDEFINED" ; break;
|
||||
case LevenbergMarquardtParams::SILENT:
|
||||
s = "SILENT";
|
||||
break;
|
||||
case LevenbergMarquardtParams::TERMINATION:
|
||||
s = "TERMINATION";
|
||||
break;
|
||||
case LevenbergMarquardtParams::LAMBDA:
|
||||
s = "LAMBDA";
|
||||
break;
|
||||
case LevenbergMarquardtParams::TRYLAMBDA:
|
||||
s = "TRYLAMBDA";
|
||||
break;
|
||||
case LevenbergMarquardtParams::TRYCONFIG:
|
||||
s = "TRYCONFIG";
|
||||
break;
|
||||
case LevenbergMarquardtParams::TRYDELTA:
|
||||
s = "TRYDELTA";
|
||||
break;
|
||||
case LevenbergMarquardtParams::DAMPED:
|
||||
s = "DAMPED";
|
||||
break;
|
||||
default:
|
||||
s = "UNDEFINED";
|
||||
break;
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
|
@ -73,10 +97,10 @@ void LevenbergMarquardtParams::print(const std::string& str) const {
|
|||
std::cout << " lambdaFactor: " << lambdaFactor << "\n";
|
||||
std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n";
|
||||
std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n";
|
||||
std::cout << " disableInnerIterations: " << disableInnerIterations << "\n";
|
||||
std::cout << " minModelFidelity: " << minModelFidelity << "\n";
|
||||
std::cout << " diagonalDamping: " << diagonalDamping << "\n";
|
||||
std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n";
|
||||
std::cout << " verbosityLM: "
|
||||
<< verbosityLMTranslator(verbosityLM) << "\n";
|
||||
std::cout.flush();
|
||||
}
|
||||
|
||||
|
|
@ -86,10 +110,10 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::increaseLambda(){
|
||||
if(params_.useFixedLambdaFactor_){
|
||||
void LevenbergMarquardtOptimizer::increaseLambda() {
|
||||
if (params_.useFixedLambdaFactor_) {
|
||||
state_.lambda *= params_.lambdaFactor;
|
||||
}else{
|
||||
} else {
|
||||
state_.lambda *= params_.lambdaFactor;
|
||||
params_.lambdaFactor *= 2.0;
|
||||
}
|
||||
|
|
@ -97,11 +121,11 @@ void LevenbergMarquardtOptimizer::increaseLambda(){
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality){
|
||||
void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {
|
||||
|
||||
if(params_.useFixedLambdaFactor_){
|
||||
if (params_.useFixedLambdaFactor_) {
|
||||
state_.lambda /= params_.lambdaFactor;
|
||||
}else{
|
||||
} else {
|
||||
// CHECK_GT(step_quality, 0.0);
|
||||
state_.lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3));
|
||||
params_.lambdaFactor = 2.0;
|
||||
|
|
@ -115,10 +139,6 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality){
|
|||
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
|
||||
const GaussianFactorGraph& linear) {
|
||||
|
||||
//Set two parameters as Ceres, will move out later
|
||||
static const double min_diagonal_ = 1e-6;
|
||||
static const double max_diagonal_ = 1e32;
|
||||
|
||||
gttic(damp);
|
||||
if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED)
|
||||
cout << "building damped system with lambda " << state_.lambda << endl;
|
||||
|
|
@ -128,7 +148,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
|
|||
state_.hessianDiagonal = linear.hessianDiagonal();
|
||||
BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) {
|
||||
for (int aa = 0; aa < v.size(); aa++) {
|
||||
v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_);
|
||||
v(aa) = std::min(std::max(v(aa), params_.min_diagonal_),
|
||||
params_.max_diagonal_);
|
||||
v(aa) = sqrt(v(aa));
|
||||
}
|
||||
}
|
||||
|
|
@ -138,21 +159,31 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
|
|||
double sigma = 1.0 / std::sqrt(state_.lambda);
|
||||
GaussianFactorGraph damped = linear;
|
||||
damped.reserve(damped.size() + state_.values.size());
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
|
||||
size_t dim = key_value.value.dim();
|
||||
Matrix A = Matrix::Identity(dim, dim);
|
||||
//Replace the identity matrix with diagonal of Hessian
|
||||
if (params_.diagonalDamping){
|
||||
if (params_.diagonalDamping) {
|
||||
BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
|
||||
// Fill in the diagonal of A with diag(hessian)
|
||||
try {
|
||||
A.diagonal() = state_.hessianDiagonal.at(key_value.key);
|
||||
Matrix A = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(
|
||||
state_.hessianDiagonal.at(key_vector.first));
|
||||
size_t dim = key_vector.second.size();
|
||||
Vector b = Vector::Zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
|
||||
damped += boost::make_shared<JacobianFactor>(key_vector.first, A, b,
|
||||
model);
|
||||
} catch (std::exception e) {
|
||||
// Don't attempt any damping if no key found in diagonal
|
||||
continue;
|
||||
}
|
||||
}
|
||||
Vector b = Vector::Zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
|
||||
damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
|
||||
} else {
|
||||
// Straightforward damping:
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
|
||||
size_t dim = key_value.value.dim();
|
||||
Matrix A = Matrix::Identity(dim, dim);
|
||||
Vector b = Vector::Zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
|
||||
damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
|
||||
}
|
||||
}
|
||||
gttoc(damp);
|
||||
return damped;
|
||||
|
|
@ -161,20 +192,22 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
|
|||
/* ************************************************************************* */
|
||||
void LevenbergMarquardtOptimizer::iterate() {
|
||||
|
||||
gttic (LM_iterate);
|
||||
gttic(LM_iterate);
|
||||
|
||||
// Pull out parameters we'll use
|
||||
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
|
||||
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;
|
||||
|
||||
// Linearize graph
|
||||
if(lmVerbosity >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
|
||||
cout << "linearizing = " << endl;
|
||||
GaussianFactorGraph::shared_ptr linear = linearize();
|
||||
|
||||
// Keep increasing lambda until we make make progress
|
||||
while (true) {
|
||||
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "trying lambda = " << state_.lambda << endl;
|
||||
|
||||
// Build damped system for this lambda (adds prior factors that make it like gradient descent)
|
||||
GaussianFactorGraph dampedSystem = buildDampedSystem(*linear);
|
||||
|
|
@ -183,9 +216,11 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
if (!params_.logFile.empty()) {
|
||||
ofstream os(params_.logFile.c_str(), ios::app);
|
||||
|
||||
boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
|
||||
boost::posix_time::ptime currentTime =
|
||||
boost::posix_time::microsec_clock::universal_time();
|
||||
|
||||
os << state_.totalNumberInnerIterations << "," << 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
|
||||
os << state_.totalNumberInnerIterations << ","
|
||||
<< 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
|
||||
<< state_.error << "," << state_.lambda << endl;
|
||||
}
|
||||
|
||||
|
|
@ -203,66 +238,73 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
try {
|
||||
delta = solve(dampedSystem, state_.values, params_);
|
||||
systemSolvedSuccessfully = true;
|
||||
} catch(IndeterminantLinearSystemException& e) {
|
||||
} catch (IndeterminantLinearSystemException& e) {
|
||||
systemSolvedSuccessfully = false;
|
||||
}
|
||||
|
||||
if(systemSolvedSuccessfully) {
|
||||
if (systemSolvedSuccessfully) {
|
||||
params_.reuse_diagonal_ = true;
|
||||
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "linear delta norm = " << delta.norm() << endl;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
|
||||
delta.print("delta");
|
||||
|
||||
// cost change in the linearized system (old - new)
|
||||
double newlinearizedError = linear->error(delta);
|
||||
|
||||
double linearizedCostChange = state_.error - newlinearizedError;
|
||||
|
||||
if(linearizedCostChange >= 0){ // step is valid
|
||||
if (linearizedCostChange >= 0) { // step is valid
|
||||
// update values
|
||||
gttic (retract);
|
||||
gttic(retract);
|
||||
newValues = state_.values.retract(delta);
|
||||
gttoc(retract);
|
||||
|
||||
// compute new error
|
||||
gttic (compute_error);
|
||||
if(lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "calculating error" << endl;
|
||||
gttic(compute_error);
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "calculating error" << endl;
|
||||
newError = graph_.error(newValues);
|
||||
gttoc(compute_error);
|
||||
|
||||
// cost change in the original, nonlinear system (old - new)
|
||||
double costChange = state_.error - newError;
|
||||
|
||||
double absolute_function_tolerance = params_.relativeErrorTol * state_.error;
|
||||
if (fabs(costChange) >= absolute_function_tolerance) {
|
||||
if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition
|
||||
// fidelity of linearized model VS original system between
|
||||
if(linearizedCostChange > 1e-15){
|
||||
modelFidelity = costChange / linearizedCostChange;
|
||||
// if we decrease the error in the nonlinear system and modelFidelity is above threshold
|
||||
step_is_successful = modelFidelity > params_.minModelFidelity;
|
||||
}else{
|
||||
step_is_successful = true; // linearizedCostChange close to zero
|
||||
}
|
||||
modelFidelity = costChange / linearizedCostChange;
|
||||
// if we decrease the error in the nonlinear system and modelFidelity is above threshold
|
||||
step_is_successful = modelFidelity > params_.minModelFidelity;
|
||||
} else {
|
||||
stopSearchingLambda = true;
|
||||
step_is_successful = true; // linearizedCostChange close to zero
|
||||
}
|
||||
|
||||
double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
|
||||
// if the change is small we terminate
|
||||
if (fabs(costChange) < minAbsoluteTolerance)
|
||||
stopSearchingLambda = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if(step_is_successful){ // we have successfully decreased the cost and we have good modelFidelity
|
||||
if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
|
||||
state_.values.swap(newValues);
|
||||
state_.error = newError;
|
||||
decreaseLambda(modelFidelity);
|
||||
break;
|
||||
}else if (!stopSearchingLambda){ // we failed to solved the system or we had no decrease in cost
|
||||
} else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "increasing lambda: old error (" << state_.error << ") new error (" << newError << ")" << endl;
|
||||
cout << "increasing lambda: old error (" << state_.error
|
||||
<< ") new error (" << newError << ")" << endl;
|
||||
increaseLambda();
|
||||
|
||||
// check if lambda is too big
|
||||
if(state_.lambda >= params_.lambdaUpperBound) {
|
||||
if(nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
|
||||
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
|
||||
if (state_.lambda >= params_.lambdaUpperBound) {
|
||||
if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
|
||||
cout
|
||||
<< "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
|
||||
<< endl;
|
||||
break;
|
||||
}
|
||||
} else { // the change in the cost is very small and it is not worth trying bigger lambdas
|
||||
|
|
@ -276,9 +318,8 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
|
||||
/* ************************************************************************* */
|
||||
LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
|
||||
LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const
|
||||
{
|
||||
if(!params.ordering)
|
||||
LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
|
||||
if (!params.ordering)
|
||||
params.ordering = Ordering::COLAMD(graph);
|
||||
return params;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -51,17 +51,19 @@ public:
|
|||
double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5)
|
||||
double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0)
|
||||
VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity
|
||||
bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed
|
||||
double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
|
||||
std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
|
||||
bool diagonalDamping; ///< if true, use diagonal of Hessian
|
||||
bool reuse_diagonal_; //an additional option in Ceres for diagonalDamping (related to efficiency)
|
||||
bool useFixedLambdaFactor_; // if true applies constant increase (or decrease) to lambda according to lambdaFactor
|
||||
bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency)
|
||||
bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor
|
||||
double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
|
||||
double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
|
||||
|
||||
LevenbergMarquardtParams() :
|
||||
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
|
||||
0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(
|
||||
1e-3), diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true) {
|
||||
0.0), verbosityLM(SILENT), minModelFidelity(1e-3),
|
||||
diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true),
|
||||
min_diagonal_(1e-6), max_diagonal_(1e32) {
|
||||
}
|
||||
virtual ~LevenbergMarquardtParams() {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -669,28 +669,30 @@ bool writeBAL(const string& filename, SfM_data &data)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
|
||||
bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){
|
||||
|
||||
SfM_data dataValues = data;
|
||||
|
||||
// Store poses or cameras in SfM_data
|
||||
Values valuesPoses = values.filter<Pose3>();
|
||||
if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
|
||||
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
|
||||
if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
|
||||
Key poseKey = symbol('x',i);
|
||||
Pose3 pose = values.at<Pose3>(poseKey);
|
||||
Cal3Bundler K = data.cameras[i].calibration();
|
||||
Cal3Bundler K = dataValues.cameras[i].calibration();
|
||||
PinholeCamera<Cal3Bundler> camera(pose, K);
|
||||
data.cameras[i] = camera;
|
||||
dataValues.cameras[i] = camera;
|
||||
}
|
||||
} else {
|
||||
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
|
||||
if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration
|
||||
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
|
||||
if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration
|
||||
for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
|
||||
Key cameraKey = i; // symbol('c',i);
|
||||
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
|
||||
data.cameras[i] = camera;
|
||||
dataValues.cameras[i] = camera;
|
||||
}
|
||||
}else{
|
||||
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
|
||||
cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
|
||||
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
|
||||
return false;
|
||||
}
|
||||
|
|
@ -698,26 +700,26 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
|
|||
|
||||
// Store 3D points in SfM_data
|
||||
Values valuesPoints = values.filter<Point3>();
|
||||
if( valuesPoints.size() != data.number_tracks()){
|
||||
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
|
||||
if( valuesPoints.size() != dataValues.number_tracks()){
|
||||
cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
|
||||
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
|
||||
}
|
||||
|
||||
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
|
||||
for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
|
||||
Key pointKey = symbol('l',j);
|
||||
if(values.exists(pointKey)){
|
||||
Point3 point = values.at<Point3>(pointKey);
|
||||
data.tracks[j].p = point;
|
||||
dataValues.tracks[j].p = point;
|
||||
}else{
|
||||
data.tracks[j].r = 1.0;
|
||||
data.tracks[j].g = 0.0;
|
||||
data.tracks[j].b = 0.0;
|
||||
data.tracks[j].p = Point3();
|
||||
dataValues.tracks[j].r = 1.0;
|
||||
dataValues.tracks[j].g = 0.0;
|
||||
dataValues.tracks[j].b = 0.0;
|
||||
dataValues.tracks[j].p = Point3();
|
||||
}
|
||||
}
|
||||
|
||||
// Write SfM_data to file
|
||||
return writeBAL(filename, data);
|
||||
return writeBAL(filename, dataValues);
|
||||
}
|
||||
|
||||
Values initialCamerasEstimate(const SfM_data& db) {
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
|
|||
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
|
||||
* @return true if the parsing was successful, false otherwise
|
||||
*/
|
||||
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);
|
||||
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values);
|
||||
|
||||
/**
|
||||
* @brief This function converts an openGL camera pose to an GTSAM camera pose
|
||||
|
|
|
|||
|
|
@ -234,7 +234,7 @@ TEST(NonlinearOptimizer, NullFactor) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(NonlinearOptimizer, MoreOptimization) {
|
||||
TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
|
||||
|
||||
NonlinearFactorGraph fg;
|
||||
fg += PriorFactor<Pose2>(0, Pose2(0, 0, 0),
|
||||
|
|
@ -284,57 +284,59 @@ TEST(NonlinearOptimizer, MoreOptimization) {
|
|||
// cout << "===================================================================================" << endl;
|
||||
|
||||
// Try LM with diagonal damping
|
||||
Values initBetter = init;
|
||||
// initBetter.insert(0, Pose2(3,4,0));
|
||||
// initBetter.insert(1, Pose2(10,2,M_PI/3));
|
||||
// initBetter.insert(2, Pose2(11,7,M_PI/2));
|
||||
Values initBetter;
|
||||
initBetter.insert(0, Pose2(3,4,0));
|
||||
initBetter.insert(1, Pose2(10,2,M_PI/3));
|
||||
initBetter.insert(2, Pose2(11,7,M_PI/2));
|
||||
|
||||
{
|
||||
// params.setDiagonalDamping(true);
|
||||
// LevenbergMarquardtOptimizer optimizer(fg, initBetter, params);
|
||||
//
|
||||
// // test the diagonal
|
||||
// GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
|
||||
// GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear);
|
||||
// VectorValues d = linear->hessianDiagonal(), //
|
||||
// expectedDiagonal = d + params.lambdaInitial * d;
|
||||
// EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal()));
|
||||
//
|
||||
// // test convergence (does not!)
|
||||
// Values actual = optimizer.optimize();
|
||||
// EXPECT(assert_equal(expected, actual));
|
||||
//
|
||||
// // Check that the gradient is zero (it is not!)
|
||||
// linear = optimizer.linearize();
|
||||
// EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
|
||||
//
|
||||
// // Check that the gradient is zero for damped system (it is not!)
|
||||
// damped = optimizer.buildDampedSystem(*linear);
|
||||
// VectorValues actualGradient = damped.gradientAtZero();
|
||||
// EXPECT(assert_equal(expectedGradient,actualGradient));
|
||||
//
|
||||
// // Check errors at convergence and errors in direction of gradient (decreases!)
|
||||
// EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5);
|
||||
// EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5);
|
||||
//
|
||||
// // Check that solve yields gradient (it's not equal to gradient, as predicted)
|
||||
// VectorValues delta = damped.optimize();
|
||||
// double factor = actualGradient[0][0]/delta[0][0];
|
||||
// EXPECT(assert_equal(actualGradient,factor*delta));
|
||||
//
|
||||
// // Still pointing downhill wrt actual gradient !
|
||||
// EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3);
|
||||
//
|
||||
// // delta.print("This is the delta value computed by LM with diagonal damping");
|
||||
//
|
||||
// // Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular)
|
||||
// EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5);
|
||||
//
|
||||
// // Check errors at convergence and errors in direction of solution (does not decrease!)
|
||||
// EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5);
|
||||
//
|
||||
// // Check errors at convergence and errors at a small step in direction of solution (does not decrease!)
|
||||
// EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3);
|
||||
params.setDiagonalDamping(true);
|
||||
LevenbergMarquardtOptimizer optimizer(fg, initBetter, params);
|
||||
|
||||
// test the diagonal
|
||||
GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
|
||||
GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear);
|
||||
VectorValues d = linear->hessianDiagonal(), //
|
||||
expectedDiagonal = d + params.lambdaInitial * d;
|
||||
EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal()));
|
||||
|
||||
// test convergence (does not!)
|
||||
Values actual = optimizer.optimize();
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
// Check that the gradient is zero (it is not!)
|
||||
linear = optimizer.linearize();
|
||||
EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
|
||||
|
||||
// Check that the gradient is zero for damped system (it is not!)
|
||||
damped = optimizer.buildDampedSystem(*linear);
|
||||
VectorValues actualGradient = damped.gradientAtZero();
|
||||
EXPECT(assert_equal(expectedGradient,actualGradient));
|
||||
|
||||
/* This block was made to test the original initial guess "init"
|
||||
// Check errors at convergence and errors in direction of gradient (decreases!)
|
||||
EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5);
|
||||
EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5);
|
||||
|
||||
// Check that solve yields gradient (it's not equal to gradient, as predicted)
|
||||
VectorValues delta = damped.optimize();
|
||||
double factor = actualGradient[0][0]/delta[0][0];
|
||||
EXPECT(assert_equal(actualGradient,factor*delta));
|
||||
|
||||
// Still pointing downhill wrt actual gradient !
|
||||
EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3);
|
||||
|
||||
// delta.print("This is the delta value computed by LM with diagonal damping");
|
||||
|
||||
// Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular)
|
||||
EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5);
|
||||
|
||||
// Check errors at convergence and errors in direction of solution (does not decrease!)
|
||||
EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5);
|
||||
|
||||
// Check errors at convergence and errors at a small step in direction of solution (does not decrease!)
|
||||
EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue