Go to the documentation of this file.
30 using namespace gtsam;
33 static const double ms = 1
e-3;
34 static const double cm = 1
e-2;
42 const double height = 0.5;
43 vector<Point3> microphones;
44 microphones.push_back(
Point3(0, 0, height));
45 microphones.push_back(
Point3(403 *
cm, 0, height));
46 microphones.push_back(
Point3(403 *
cm, 403 *
cm, height));
47 microphones.push_back(
Point3(0, 403 *
cm, 2 * height));
69 size_t K = microphones.size();
70 vector<double> simulatedTOA(
K);
71 for (
size_t i = 0;
i <
K;
i++) {
79 vector<vector<double>>
simulateTOA(
const vector<Point3>& microphones,
81 vector<vector<double>> simulatedTOA;
83 simulatedTOA.push_back(
simulateTOA(microphones, event));
91 const vector<vector<double>>& simulatedTOA) {
95 auto model = noiseModel::Isotropic::Sigma(1, 0.5 *
ms);
97 size_t K = microphones.size();
99 for (
auto toa : simulatedTOA) {
100 for (
size_t i = 0;
i <
K;
i++) {
121 int main(
int argc,
char* argv[]) {
124 size_t K = microphones.size();
125 for (
size_t i = 0;
i <
K;
i++) {
126 cout <<
"mic" <<
i <<
" = " << microphones[
i] << endl;
134 auto simulatedTOA =
simulateTOA(microphones, groundTruth);
136 for (
size_t i = 0;
i <
K;
i++) {
137 cout <<
"z_" <<
key <<
i <<
" = " << simulatedTOA[
key][
i] /
ms <<
" ms"
147 initialEstimate.print(
"Initial Estimate:\n");
151 params.setAbsoluteErrorTol(1
e-10);
152 params.setVerbosityLM(
"SUMMARY");
virtual const Values & optimize()
Array< double, 1, 3 > e(1./3., 0.5, 2.)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
int main(int argc, char *argv[])
static const TimeOfArrival kTimeOfArrival(330)
static const SmartProjectionParams params
Time of arrival to given sensor.
static const double timeOfEvent
Common expressions, both linear and non-linear.
EIGEN_DONT_INLINE Scalar zero()
A "Time of Arrival" factor - so little code seems hardly worth it :-)
vector< Event > createTrajectory(size_t n)
vector< Point3 > defineMicrophones()
noiseModel::Diagonal::shared_ptr model
const gtsam::Symbol key('X', 0)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Factor Graph consisting of non-linear factors.
def trajectory(g0, g1, N=20)
NonlinearFactorGraph createGraph(const vector< Point3 > µphones, const vector< vector< double >> &simulatedTOA)
vector< double > simulateTOA(const vector< Point3 > µphones, const Event &event)
Values createInitialEstimate(size_t n)
NonlinearFactorGraph graph
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:09