diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 79a54903e..d298091dc 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -8,7 +8,10 @@ public: Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const { - if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); + const Rot2& R = q.rotation(); + if (H) (*H) = (gtsam::Matrix(2, 3) << + R.c(), -R.s(), 0.0, + R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } }; diff --git a/doc/gtsam.lyx b/doc/gtsam.lyx index 29be8dbe4..a5adc2b60 100644 --- a/doc/gtsam.lyx +++ b/doc/gtsam.lyx @@ -1,7 +1,9 @@ -#LyX 2.1 created this file. For more info see http://www.lyx.org/ -\lyxformat 474 +#LyX 2.2 created this file. For more info see http://www.lyx.org/ +\lyxformat 508 \begin_document \begin_header +\save_transient_properties true +\origin unavailable \textclass article \begin_preamble \usepackage{times} @@ -50,16 +52,16 @@ \language_package default \inputencoding auto \fontencoding T1 -\font_roman ae -\font_sans default -\font_typewriter default -\font_math auto +\font_roman "ae" "default" +\font_sans "default" "default" +\font_typewriter "default" "default" +\font_math "auto" "auto" \font_default_family rmdefault \use_non_tex_fonts false \font_sc false \font_osf false -\font_sf_scale 100 -\font_tt_scale 100 +\font_sf_scale 100 100 +\font_tt_scale 100 100 \graphics default \default_output_format default \output_sync 0 @@ -1061,7 +1063,7 @@ noindent \begin_layout Subsection \begin_inset CommandInset label LatexCommand label -name "sub:Full-Posterior-Inference" +name "subsec:Full-Posterior-Inference" \end_inset @@ -1272,7 +1274,7 @@ ing to odometry measurements. (see Section \begin_inset CommandInset ref LatexCommand ref -reference "sub:Full-Posterior-Inference" +reference "subsec:Full-Posterior-Inference" \end_inset @@ -1469,7 +1471,7 @@ GPS-like \begin_inset CommandInset include LatexCommand lstinputlisting filename "Code/LocalizationFactor.cpp" -lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationFactor},language={C++},numbers=left" +lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/LocalizationExample.cpp},label={listing:LocalizationFactor}" \end_inset @@ -1590,15 +1592,15 @@ q_{y} \begin_inset Formula $2\times3$ \end_inset - matrix: + matrix in tangent space which is the same the as the rotation matrix: \end_layout \begin_layout Standard \begin_inset Formula \[ H=\left[\begin{array}{ccc} -1 & 0 & 0\\ -0 & 1 & 0 +\cos(q_{\theta}) & -\sin(q_{\theta}) & 0\\ +\sin(q_{\theta}) & \cos(q_{\theta}) & 0 \end{array}\right] \] @@ -1750,7 +1752,7 @@ global The marginals can be recovered exactly as in Section \begin_inset CommandInset ref LatexCommand ref -reference "sub:Full-Posterior-Inference" +reference "subsec:Full-Posterior-Inference" \end_inset @@ -1770,7 +1772,7 @@ filename "Code/LocalizationOutput5.txt" Comparing this with the covariance matrices in Section \begin_inset CommandInset ref LatexCommand ref -reference "sub:Full-Posterior-Inference" +reference "subsec:Full-Posterior-Inference" \end_inset diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf index 240528179..c6a39a79c 100644 Binary files a/doc/gtsam.pdf and b/doc/gtsam.pdf differ diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 33fe96ed8..d9b205359 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -84,15 +84,16 @@ class UnaryFactor: public NoiseModelFactor1 { // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. - Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const override { - // The measurement function for a GPS-like measurement is simple: - // error_x = pose.x - measurement.x - // error_y = pose.y - measurement.y - // Consequently, the Jacobians are: - // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] - // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] - if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished(); + Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const override { + // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta] + // The error is then simply calculated as E(q) = h(q) - m: + // error_x = q.x - mx + // error_y = q.y - my + // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix + // H = [ cos(q.theta) -sin(q.theta) 0 ] + // [ sin(q.theta) cos(q.theta) 0 ] + const Rot2& R = q.rotation(); + if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); }