Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding lever-arm to GPS factors #1975

Merged
merged 10 commits into from
Jan 17, 2025
Merged

Adding lever-arm to GPS factors #1975

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
5254b4f
adding lever arm optional argument to gps factors
mnissov Jan 13, 2025
57b8f6b
fix GPSFactor serialization
mnissov Jan 14, 2025
7efcb46
update GPSFactor interface
mnissov Jan 14, 2025
af1e6e3
reverting changes to existing factors
mnissov Jan 15, 2025
b815936
creating GPSFactorArm and GPSFactor2Arm w/ lever arm arguments
mnissov Jan 15, 2025
94e96ab
remove EstimateState from GPSFactorArm
mnissov Jan 15, 2025
4737f0d
improving comments
mnissov Jan 16, 2025
b5349a7
improved documentation, params and getters
mnissov Jan 16, 2025
0c13038
typo in print for GPSFactor2Arm
mnissov Jan 16, 2025
d1106ae
adding GPSFactors with lever arm as state
mnissov Jan 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
125 changes: 125 additions & 0 deletions gtsam/navigation/GPSFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,69 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,

return make_pair(nTb, nV);
}

//***************************************************************************
void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(bL_, e->bL_, tol);
}

//***************************************************************************
Vector GPSFactorArm::evaluateError(const Pose3& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.rotation().matrix();
if (H) {
H->resize(3, 6);

H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
}

return p.translation() + nRb * bL_ - nT_;
dellaert marked this conversation as resolved.
Show resolved Hide resolved
}

//***************************************************************************
void GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}

//***************************************************************************
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.rotation().matrix();
if (H1) {
H1->resize(3, 6);

H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}

return p.translation() + nRb * bL - nT_;
}

//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
Expand All @@ -85,5 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p,
}

//***************************************************************************
void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(bL_, e->bL_, tol);
}

//***************************************************************************
Vector GPSFactor2Arm::evaluateError(const NavState& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.attitude().matrix();
if (H) {
dellaert marked this conversation as resolved.
Show resolved Hide resolved
H->resize(3, 9);

H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
H->block<3, 3>(0, 6).setZero();
}

return p.position() + nRb * bL_ - nT_;
}

//***************************************************************************
void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}

//***************************************************************************
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.attitude().matrix();
if (H1) {
H1->resize(3, 9);

H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
H1->block<3, 3>(0, 6).setZero();
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}

return p.position() + nRb * bL - nT_;
}

}/// namespace gtsam
Loading
Loading