diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 45817a3f9..315261293 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -119,7 +119,6 @@ public: else if (kind == Leaf) std::cout << indent << "Leaf, key = " << content.key << std::endl; else if (kind == Function) { - std::cout << indent << "Function" << std::endl; content.ptr->print(indent + " "); } } diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d8f9cf3ff..73852cee8 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -211,8 +211,16 @@ struct Jacobian { typedef Eigen::Matrix::dimension, traits::dimension> type; }; -// Eigen format for printing Jacobians -static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); +// Helper function for printing Jacobians with compact Eigen format, and trace +template +static void PrintJacobianAndTrace(const std::string& indent, + const typename Jacobian::type& dTdA, + const ExecutionTrace trace) { + static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << indent << "d(" << typeid(T).name() << ")/d(" << typeid(A).name() + << ") = " << dTdA.format(kMatlabFormat) << std::endl; + trace.print(indent); +} //----------------------------------------------------------------------------- /// Unary Function Expression @@ -268,8 +276,7 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "UnaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); std::cout << indent << "}" << std::endl; } @@ -388,10 +395,8 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "BinaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); - std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; - trace2.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); std::cout << indent << "}" << std::endl; } @@ -502,12 +507,9 @@ public: /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "TernaryExpression::Record {" << std::endl; - std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; - trace1.print(indent); - std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; - trace2.print(indent); - std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; - trace3.print(indent); + PrintJacobianAndTrace(indent, dTdA1, trace1); + PrintJacobianAndTrace(indent, dTdA2, trace2); + PrintJacobianAndTrace(indent, dTdA3, trace3); std::cout << indent << "}" << std::endl; }