Skip to content

Commit 0db7489

Browse files
watsonryanwatsonryan
committed
DCS now working -- had to write new DCS factor because GTSAM's implementation does not seem to work properly
1 parent f97ada6 commit 0db7489

File tree

4 files changed

+312
-13
lines changed

4 files changed

+312
-13
lines changed

‎3rdparty/RobustGNSS/gtsam/gtsam/gnssNavigation/GNSSDCSFactor.cpp

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
/**
2+
* @file GNSSDCSFactor.cpp
3+
* @author Ryan Watson
4+
* @brief Implementation file for GNSS factor With mulit-modal uncert. model
5+
**/
6+
7+
#include <gtsam/gnssNavigation/GNSSDCSFactor.h>
8+
9+
using namespace std;
10+
using namespace boost;
11+
using namespace merge;
12+
13+
namespace gtsam {
14+
//***************************************************************************
15+
Vector GNSSDCSFactor::whitenedError(const gtsam::Values& x,
16+
boost::optional<std::vector<Matrix>&> H) const {
17+
18+
19+
const nonBiasStates& q = x.at<nonBiasStates>(k1_);
20+
const phaseBias& g = x.at<phaseBias>(k2_);
21+
22+
Vector h = obsMap(satXYZ_, nomXYZ_, 1);
23+
Matrix gnssPartials = Z_1x1;
24+
25+
double res_range = (h.transpose() * q) - measured_[0];
26+
double res_phase = (h.transpose() * q) + g[0] - measured_[1];
27+
28+
Eigen::VectorXd res(2);
29+
res << res_range, res_phase;
30+
31+
if (H) {
32+
Matrix H_g(2,5);
33+
H_g.row(0) = h;
34+
H_g.row(1) = h;
35+
36+
Matrix H_b(2,1);
37+
H_b(0,0) = 0.0;
38+
H_b(1,0) = 1.0;
39+
40+
(*H)[0].resize(H_g.rows(), H_g.cols());
41+
(*H)[1].resize(H_b.rows(), H_b.cols());
42+
43+
(*H)[0] = H_g;
44+
(*H)[1] = H_b;
45+
46+
}
47+
48+
49+
// DCS Base scaling of cov.
50+
double v_range, v_phase;
51+
52+
// check range residaul
53+
if (std::pow(res(0),2) < k_(0) || iter_count_ < 2)
54+
{
55+
v_range = model_(0,0);
56+
}
57+
else
58+
{
59+
double scale = (4*std::pow(k_(0),2)) / (std::pow( std::pow(res(0),2) + k_(0),2));
60+
v_range = model_(0,0)/scale;
61+
}
62+
63+
// check phase residaul
64+
if (std::pow(res(1),2) < k_(1) || iter_count_ < 2)
65+
{
66+
v_phase = model_(1,1);
67+
}
68+
else
69+
{
70+
double scale = (4*std::pow(k_(1),2)) / (std::pow( std::pow(res(1),2) + k_(1),2));
71+
v_phase = model_(1,1)/scale;
72+
}
73+
74+
75+
Eigen::MatrixXd cov_model(2,2);
76+
cov_model << v_range, 0.0, 0.0, v_phase;
77+
78+
return (gtsam::noiseModel::Gaussian::Covariance(cov_model))->whiten(res);
79+
}
80+
81+
Vector GNSSDCSFactor::unwhitenedError(const gtsam::Values& x,
82+
boost::optional<std::vector<Matrix>&> H) const {
83+
84+
const nonBiasStates& q = x.at<nonBiasStates>(k1_);
85+
const phaseBias& g = x.at<phaseBias>(k2_);
86+
87+
Vector h = obsMap(satXYZ_, nomXYZ_, 1);
88+
Matrix gnssPartials = Z_1x1;
89+
90+
double res_range = (h.transpose() * q) - measured_[0];
91+
double res_phase = (h.transpose() * q) + g[0] - measured_[1];
92+
93+
Eigen::VectorXd res(2);
94+
res << res_range, res_phase;
95+
96+
if (H) {
97+
98+
Matrix H_g(2,5);
99+
H_g.row(0) = h;
100+
H_g.row(1) = h;
101+
102+
Matrix H_b(2,1);
103+
H_b(0,0) = 0.0;
104+
H_b(1,0) = 1.0;
105+
106+
(*H)[0].resize(H_g.rows(), H_g.cols());
107+
(*H)[1].resize(H_b.rows(), H_b.cols());
108+
109+
(*H)[0] = H_g;
110+
(*H)[1] = H_b;
111+
112+
}
113+
114+
return res;
115+
}
116+
117+
} //namespace

‎3rdparty/RobustGNSS/gtsam/gtsam/gnssNavigation/GNSSDCSFactor.h

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,182 @@
1+
/**
2+
* @file GNSSDCSFactor.h
3+
* @author Ryan Watson
4+
* @brief Header file for GNSS Factor with multimodal uncert. model
5+
**/
6+
7+
#pragma once
8+
#include <gtsam/base/Vector.h>
9+
#include <gtsam/base/Matrix.h>
10+
#include <gtsam/base/Testable.h>
11+
#include <gtsam/geometry/Pose3.h>
12+
#include <gtsam/geometry/Point2.h>
13+
#include <gtsam/geometry/Point3.h>
14+
#include <gtsam/linear/GaussianFactor.h>
15+
#include <gtsam/gnssNavigation/GnssTools.h>
16+
#include <gtsam/nonlinear/NonlinearFactor.h>
17+
#include <gtsam/gnssNavigation/nonBiasStates.h>
18+
19+
20+
#include <libcluster/merge.h>
21+
#include <libcluster/probutils.h>
22+
#include <libcluster/libcluster.h>
23+
#include <libcluster/distributions.h>
24+
25+
#include <boost/tuple/tuple.hpp>
26+
#include <boost/math/special_functions.hpp>
27+
28+
29+
namespace gtsam {
30+
31+
32+
class GTSAM_EXPORT GNSSDCSFactor : public NonlinearFactor {
33+
34+
private:
35+
36+
typedef gtsam::NonlinearFactor Base;
37+
typedef GNSSDCSFactor This;
38+
39+
mutable int iter_count_;
40+
Key k1_,k2_;
41+
Point3 satXYZ_;
42+
Point3 nomXYZ_;
43+
nonBiasStates h_;
44+
Vector2 measured_, k_;
45+
Eigen::MatrixXd model_;
46+
47+
public:
48+
49+
typedef boost::shared_ptr<GNSSDCSFactor> shared_ptr;
50+
51+
GNSSDCSFactor() : measured_() {
52+
h_=Matrix(2,5);
53+
}
54+
55+
GNSSDCSFactor(Key deltaStates, Key bias, const Vector2 measurement,
56+
const Point3 satXYZ, const Point3 nomXYZ, Eigen::MatrixXd &model, const Vector2 k) :
57+
Base(cref_list_of<2>(deltaStates)(bias)), k1_(deltaStates), k2_(bias), k_(k), measured_(measurement), satXYZ_(satXYZ), nomXYZ_(nomXYZ), model_(model), iter_count_(0) {
58+
}
59+
60+
virtual ~GNSSDCSFactor() {
61+
}
62+
63+
/** print */
64+
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
65+
std::cout << s << "GNSS Factor("
66+
<< keyFormatter(k1_) << ","
67+
<< keyFormatter(k2_) << ")\n"
68+
<< " measured: " << measured_.transpose();
69+
}
70+
71+
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
72+
const This *t = dynamic_cast<const This*>(&f);
73+
74+
if (t && Base::equals(f)) { return k1_ == t->k1_ && k2_ == t->k2_; }
75+
else{ return false; }
76+
}
77+
78+
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
79+
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
80+
gtsam::NonlinearFactor::shared_ptr(new GNSSDCSFactor(*this)));
81+
}
82+
83+
virtual double error(const gtsam::Values& x) const {
84+
return whitenedError(x).squaredNorm();
85+
}
86+
87+
Vector unwhitenedError(const gtsam::Values& x,
88+
boost::optional<std::vector<Matrix>&> H = boost::none) const;
89+
90+
Vector whitenedError(const gtsam::Values& x,
91+
boost::optional<std::vector<Matrix>&> H = boost::none) const;
92+
93+
virtual Vector residual(const gtsam::Values& x) const {
94+
const Vector b = unwhitenedError(x);
95+
return b;
96+
}
97+
98+
virtual size_t dim() const {
99+
return 5;
100+
}
101+
102+
std::size_t size() const {
103+
return 2;
104+
}
105+
106+
bool active(const gtsam::Values& x) const {
107+
return true;
108+
}
109+
110+
111+
112+
/* ************************************************************************* */
113+
/**
114+
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
115+
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
116+
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
117+
*/
118+
/* This version of linearize recalculates the noise model each time */
119+
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
120+
const gtsam::Values& x) const {
121+
122+
iter_count_ += 1;
123+
124+
if (!active(x))
125+
return boost::shared_ptr<JacobianFactor>();
126+
127+
// Call evaluate error to get Jacobians and RHS vector b
128+
std::vector<Matrix> A(this->size());
129+
Vector b = unwhitenedError(x, A);
130+
131+
// Fill in terms, needed to create JacobianFactor below
132+
std::vector<std::pair<Key, Matrix> > terms(size());
133+
for (size_t j = 0; j < size(); ++j) {
134+
terms[j].first = keys()[j];
135+
terms[j].second.swap(A[j]);
136+
}
137+
138+
// DCS Base scaling of cov.
139+
double v_range, v_phase;
140+
141+
// check range residaul
142+
if (std::pow(b(0),2) < k_(0) || iter_count_ < 2)
143+
{
144+
v_range = model_(0,0);
145+
}
146+
else
147+
{
148+
double scale = (4*std::pow(k_(0),2)) / (std::pow( std::pow(b(0),2) + k_(0),2));
149+
v_range = model_(0,0)/scale;
150+
}
151+
152+
// check phase residaul
153+
if (std::pow(b(1),2) < k_(1) || iter_count_ < 2)
154+
{
155+
v_phase = model_(1,1);
156+
}
157+
else
158+
{
159+
double scale = (4*std::pow(k_(1),2)) / (std::pow( std::pow(b(1),2) + k_(1),2));
160+
v_phase = model_(1,1)/scale;
161+
}
162+
163+
auto jacobianFactor = GaussianFactor::shared_ptr( new JacobianFactor(terms, -b, noiseModel::Diagonal::Variances((gtsam::Vector(2) << v_range, v_phase).finished()) ));
164+
165+
return jacobianFactor;
166+
}
167+
168+
169+
private:
170+
171+
/** Serialization function */
172+
friend class boost::serialization::access;
173+
template<class ARCHIVE>
174+
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
175+
ar
176+
& boost::serialization::make_nvp("NonlinearFactor",
177+
boost::serialization::base_object<Base>(*this));
178+
ar & BOOST_SERIALIZATION_NVP(measured_);
179+
}
180+
181+
}; // GNSSDCSFactor Factor
182+
} // namespace

‎examples/build_examples.sh

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,13 @@ BDIR="$CURRDIR/build"
1414
# Build example files
1515
#--------------------------------------------------------------------------------------------------
1616

17-
# g++ test_gnss_bce.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -lboost_system -lboost_program_options -ltbb -lcluster -Wno-deprecated-declarations -lgtsam -fopenmp -o "$BDIR/test_gnss_bce"
18-
1917
g++ test_gnss_ice.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -ltbb -ltbbmalloc -lboost_system -lboost_program_options -lgpstk -lcluster -Wno-deprecated-declarations -lgtsam -fopenmp -o "$BDIR/test_gnss_ice"
2018

21-
g++ test_gnss_l2.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -lboost_system -lboost_program_options -ltbb -lgpstk -lgtsam -lcluster -Wno-deprecated-declarations -fopenmp -o "$BDIR/test_gnss_l2"
19+
g++ test_gnss_l2.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -ltbb -ltbbmalloc -lboost_system -lboost_program_options -lgpstk -lcluster -Wno-deprecated-declarations -lgtsam -fopenmp -o "$BDIR/test_gnss_l2"
20+
21+
g++ test_gnss_maxmix.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -ltbb -ltbbmalloc -lboost_system -lboost_program_options -lgpstk -lcluster -Wno-deprecated-declarations -lgtsam -fopenmp -o "$BDIR/test_gnss_maxmix"
2222

23-
g++ test_gnss_maxmix.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -lboost_system -lboost_program_options -ltbb -lgpstk -lgtsam -lcluster -Wno-deprecated-declarations -fopenmp -o "$BDIR/test_gnss_maxmix"
23+
g++ test_gnss_dcs.cpp -std=c++11 -I"$EDIR" -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -ltbb -ltbbmalloc -lboost_system -lboost_program_options -lgpstk -lcluster -Wno-deprecated-declarations -lgtsam -fopenmp -o "$BDIR/test_gnss_dcs"
2424

2525

2626
g++ rnx_2_gtsam.cpp -std=c++11 -L"$LDIR" -Wl,-rpath="$LDIR" -I"$IDIR" -lboost_system -lboost_program_options -ltbb -Wno-deprecated-declarations -lgpstk -fopenmp -o "$BDIR/rnx_2_gtsam"

‎examples/test_gnss_dcs.cpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@
1212
#include <gtsam/slam/BetweenFactor.h>
1313
#include <gtsam/gnssNavigation/GnssData.h>
1414
#include <gtsam/gnssNavigation/GnssTools.h>
15+
#include <gtsam/gnssNavigation/GNSSDCSFactor.h>
1516
#include <gtsam/gnssNavigation/nonBiasStates.h>
1617
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
17-
#include <gtsam/gnssNavigation/GNSSMultiModalFactor.h>
1818
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
1919

2020

@@ -158,18 +158,18 @@ int main(int argc, char* argv[])
158158

159159
noiseModel::Diagonal::shared_ptr initNoise = noiseModel::Diagonal::Variances((gtsam::Vector(1) << 3e6).finished());
160160

161+
// set dcs kernel with as 3-sigma from inlier cov model
162+
Vector2 kernelWidth;
163+
kernelWidth << rangeWeight*3.0, phaseWeight*3.0;
164+
161165
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
162166

163167
residuals.setZero(1000,2);
164168

165-
// For the dcs model, we will use a mixture model with 1 component
166-
Eigen::RowVectorXd m(2);
167-
m << 0.0, 0.0;
169+
// For the dcs, make robust GTSAM noise model
170+
Eigen::MatrixXd cov_model(2,2);
171+
cov_model << std::pow(rangeWeight,2), 0.0, 0.0, std::pow(phaseWeight,2);
168172

169-
// Add component 1. (i.e., the assumed error covariance)
170-
Eigen::MatrixXd c(2,2);
171-
c<< std::pow(rangeWeight,2), 0.0, 0.0, std::pow(phaseWeight,2);
172-
globalMixtureModel.push_back(boost::make_tuple(0, 0, 0.0, m, c));
173173

174174
int lastStep = get<0>(data.back());
175175

@@ -212,7 +212,7 @@ int main(int argc, char* argv[])
212212
++factor_count;
213213
}
214214

215-
graph->add(boost::make_shared<GNSSMultiModalFactor>(X(currKey), G(bias_counter[svn]), obs, satXYZ, nomXYZ, globalMixtureModel));
215+
graph->add(boost::make_shared<GNSSDCSFactor>(X(currKey), G(bias_counter[svn]), obs, satXYZ, nomXYZ, cov_model, kernelWidth));
216216

217217
prn_vec.push_back(svn);
218218
factor_count_vec.push_back(++factor_count);

0 commit comments

Comments
 (0)