12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

use "const double&" in evaluateError

release/4.3a0
krunalchande 2014-11-19 13:11:47 -05:00
parent 881ecebfc9
commit 2da3a1ab21
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/MagFactor.h
View File

@ -74,7 +74,7 @@ public:
*/ */
Vector evaluateError(const Rot2& nRb, Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b // measured bM = nRb<EFBFBD> * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_; Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
@ -112,7 +112,7 @@ public:
*/ */
Vector evaluateError(const Rot3& nRb, Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b // measured bM = nRb<EFBFBD> * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
@ -151,7 +151,7 @@ public:
Vector evaluateError(const Point3& nM, const Point3& bias, Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRb<EFBFBD> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2) if (H2)
*H2 = eye(3); *H2 = eye(3);
@ -189,11 +189,11 @@ public:
* @param nM (unknown) local earth magnetic field vector, in nav frame * @param nM (unknown) local earth magnetic field vector, in nav frame
* @param bias (unknown) 3D bias * @param bias (unknown) 3D bias
*/ */
Vector evaluateError(double scale, const Unit3& direction, Vector evaluateError(const double& scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none, const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const { boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRb<EFBFBD> * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Point3 hx = scale * rotated.point3() + bias; Point3 hx = scale * rotated.point3() + bias;
if (H1) if (H1)