39 Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
40 Vector e = nZ_.error(nRef, D_e_nRef);
41 (*H) = D_e_nRef * D_nRef_R;
48 Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
56 void Rot3GravityFactor::print(
const string& s,
58 cout <<
s <<
"Rot3GravityFactor on " << keyFormatter(this->
key()) <<
"\n";
59 nZ_.print(
" measured direction in nav frame: ");
60 bRef_.print(
" reference direction in body frame: ");
61 this->noiseModel_->print(
" noise model: ");
68 return e !=
NULL && Base::equals(*
e,
tol) && this->nZ_.equals(
e->nZ_,
tol)
69 && this->bRef_.equals(
e->bRef_,
tol);
73 void Pose3GravityFactor::print(
const string& s,
75 cout <<
s <<
"Pose3GravityFactor on " << keyFormatter(this->
key()) <<
"\n";
76 nZ_.print(
" measured direction in nav frame: ");
77 bRef_.print(
" reference direction in body frame: ");
78 this->noiseModel_->print(
" noise model: ");
85 return e !=
NULL && Base::equals(*
e,
tol) && this->nZ_.equals(
e->nZ_,
tol)
86 && this->bRef_.equals(
e->bRef_,
tol);