gtsam/gtsam/nonlinear/NonlinearISAM.h

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * NonlinearISAM.h
 *
 *  Created on: Jan 19, 2010
 *      Author: Viorela Ila and Richard Roberts
 */

#pragma once

#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianISAM.h>

namespace gtsam {
/**
 * Wrapper class to manage ISAM in a nonlinear context
 */
template<class Values>
class NonlinearISAM {
public:

	typedef gtsam::NonlinearFactorGraph<Values> Factors;

protected:

	/** The internal iSAM object */
	gtsam::GaussianISAM isam_;

	/** The current linearization point */
	Values linPoint_;

	/** The ordering */
	gtsam::Ordering ordering_;

	/** The original factors, used when relinearizing */
	Factors factors_;

	/** The reordering interval and counter */
	int reorderInterval_;
	int reorderCounter_;

public:

	/** default constructor will disable periodic reordering */
	NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {}

	/** Periodically reorder and relinearize */
	NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}

	/** Add new factors along with their initial linearization points */
	void update(const Factors& newFactors, const Values& initialValues);

	/** Return the current solution estimate */
	Values estimate();

	/** Relinearization and reordering of variables */
	void reorder_relinearize();

	// access

	/** Return the current linearization point */
	const Values& getLinearizationPoint() { return linPoint_; }

	/** Get the ordering */
	const gtsam::Ordering& getOrdering() const { return ordering_; }

	/** get underlying nonlinear graph */
	const Factors& getFactorsUnsafe() { return factors_; }

	/** get counters */
	int reorderInterval() const { return reorderInterval_; }
	int reorderCounter() const { return reorderCounter_; }
};

} // \namespace gtsam