diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 5565b6206..c5c2efc66 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -9,6 +9,13 @@ * -------------------------------------------------------------------------- */ +/** + * @file Pose2SLAMwSPCG.cpp + * @brief A 2D Pose SLAM example using the SimpleSPCGSolver. + * @author Yong-Dian Jian + * @date June 2, 2012 + */ + #include #include #include @@ -55,6 +62,9 @@ int main(void) { cout << "initial error = " << graph.error(initialEstimate) << endl ; // 4. Single Step Optimization using Levenberg-Marquardt + // Note: Although there are many options in IterativeOptimizationParameters, + // the SimpleSPCGSolver doesn't actually use all of them at this moment. + // More detail in the next release. LevenbergMarquardtParams param; param.linearSolverType = SuccessiveLinearizationParams::CG; param.iterativeParams = boost::make_shared(); @@ -63,6 +73,5 @@ int main(void) { Values result = optimizer.optimize(); cout << "final error = " << graph.error(result) << endl; - return 0 ; + return 0 ; } - diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h index c7367f663..04729fc1a 100644 --- a/gtsam/linear/SimpleSPCGSolver.h +++ b/gtsam/linear/SimpleSPCGSolver.h @@ -19,12 +19,19 @@ namespace gtsam { /** - * This class gives a simple implementation to the SPCG solver presented in Dellaert et al. IROS'10. - * Given a linear least-squares problem f(x) = |A x - b|^2. - * We split the problem into f(x) = |At - bt|^2 + |Ac - bc|^2 where At denotes the "tree" part, - * and Ac denotes the "constraint" part. At is factorized into Qt Rt, and we compute ct = Qt\bt, and xt = Rt\ct. - * Then we solve reparametrized problem f(y) = |y|^2 + |Ac Rt \ y - by|^2, where y = Rt(x - xt), and by = (bc - Ac xt) - * In the matrix form, it is equivalent to solving [Ac Rt^{-1} ; I ] y = [by ; 0]. + * This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10. + * + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into + * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. + * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it + * with the least-squares variation of the conjugate gradient method. + * + * Note: A full SPCG implementation will come up soon in the next release. + * + * \nosubgrouping */ class SimpleSPCGSolver : public IterativeSolver { @@ -36,19 +43,15 @@ public: protected: - size_t nVar_ ; /* number of variables */ - size_t nAc_ ; /* number of factors in Ac */ - - /* for SPCG */ - FactorGraph::shared_ptr Ac_; - GaussianBayesNet::shared_ptr Rt_; - VectorValues::shared_ptr xt_; - VectorValues::shared_ptr y0_; - VectorValues::shared_ptr by_; /* [bc_bar ; 0 ] */ - - /* buffer for row and column vectors in */ - VectorValues::shared_ptr tmpY_; - VectorValues::shared_ptr tmpB_; + size_t nVar_ ; ///< number of variables \f$ x \f$ + size_t nAc_ ; ///< number of factors in \f$ A_c \f$ + FactorGraph::shared_ptr Ac_; ///< the constrained part of the graph + GaussianBayesNet::shared_ptr Rt_; ///< the gaussian bayes net of the tree part of the graph + VectorValues::shared_ptr xt_; ///< the solution of the \f$ A_t^{-1} b_t \f$ + VectorValues::shared_ptr y0_; ///< a column zero vector + VectorValues::shared_ptr by_; ///< \f$ [\bar{b_y} ; 0 ] \f$ + VectorValues::shared_ptr tmpY_; ///< buffer for the column vectors + VectorValues::shared_ptr tmpB_; ///< buffer for the row vectors public: @@ -60,25 +63,25 @@ protected: VectorValues::shared_ptr optimize (const VectorValues &initial); - /* output = [bc_bar ; 0 ] - [Ac Rt^{-1} ; I] y */ + /** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */ void residual(const VectorValues &input, VectorValues &output); - /* output = [Ac Rt^{-1} ; I] input */ + /** output = \f$ [A_c R_t^{-1} ; I] \f$ input */ void multiply(const VectorValues &input, VectorValues &output); - /* output = [Rt^{-T} Ac^T, I] input */ + /** output = \f$ [R_t^{-T} A_c^T, I] \f$ input */ void transposeMultiply(const VectorValues &input, VectorValues &output); - /* output = Rt^{-1} input */ + /** output = \f$ R_t^{-1} \f$ input */ void backSubstitute(const VectorValues &rhs, VectorValues &sol) ; - /* output = Rt^{-T} input */ + /** output = \f$ R_t^{-T} \f$ input */ void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ; - /* return x = Rt^{-1} y + x_t */ + /** return \f$ R_t^{-1} y + x_t \f$ */ VectorValues::shared_ptr transform(const VectorValues &y); - /* naively split a gaussian factor graph into tree and constraint parts + /** naively split a gaussian factor graph into tree and constraint parts * Note: This function has to be refined for your graph/application */ boost::tuple::shared_ptr> splitGraph(const GaussianFactorGraph &gfg);