mag_cal.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017 Daniel Koch and James Jackson, BYU MAGICC Lab.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
38 #ifndef ROSFLIGHT_SENSORS_CALBRATE_MAG_H
39 #define ROSFLIGHT_SENSORS_CALBRATE_MAG_H
40 
42 #include <ros/ros.h>
43 
44 #include <rosflight_msgs/ParamSet.h>
45 
46 #include <sensor_msgs/MagneticField.h>
47 
48 #include <cmath>
49 #include <eigen3/Eigen/Eigen>
50 #include <random>
51 
52 #include <boost/bind.hpp>
53 
55 #include <vector>
56 
57 namespace rosflight
58 {
63 {
64 public:
65  CalibrateMag();
66 
67  void run();
68 
72  void start_mag_calibration();
73 
74  void do_mag_calibration();
75 
80  bool mag_callback(const sensor_msgs::MagneticField::ConstPtr &mag);
81 
82  void set_reference_magnetic_field_strength(double reference_magnetic_field);
83 
88  bool is_calibrating() { return calibrating_; }
89 
91  const double a11() const { return A_(0, 0); }
92  const double a12() const { return A_(0, 1); }
93  const double a13() const { return A_(0, 2); }
94  const double a21() const { return A_(1, 0); }
95  const double a22() const { return A_(1, 1); }
96  const double a23() const { return A_(1, 2); }
97  const double a31() const { return A_(2, 0); }
98  const double a32() const { return A_(2, 1); }
99  const double a33() const { return A_(2, 2); }
100  const double bx() const { return b_(0, 0); }
101  const double by() const { return b_(1, 0); }
102  const double bz() const { return b_(2, 0); }
103 
104 private:
105  bool set_param(std::string name, double value);
106 
109 
111 
114 
115  Eigen::MatrixXd A_, b_;
116 
118 
120  bool first_time_;
122  double start_time_;
126  double inlier_thresh_;
127  Eigen::Vector3d measurement_prev_;
129 
130  // function to perform RANSAC on ellipsoid data
131  Eigen::MatrixXd ellipsoidRANSAC(EigenSTL::vector_Vector3d meas, int iters, double inlier_thresh);
132 
133  // function to vector from ellipsoid center to surface along input vector
134  Eigen::Vector3d intersect(Eigen::Vector3d r_m, Eigen::Vector3d r_e, Eigen::MatrixXd Q, Eigen::MatrixXd ub, double k);
135 
136  /*
137  sort eigenvalues and eigenvectors output from Eigen library
138  */
139  void eigSort(Eigen::MatrixXd &w, Eigen::MatrixXd &v);
140 
141  /*
142  This function gets ellipsoid parameters via least squares on ellipsoidal data
143  according to the paper: Li, Qingde, and John G. Griffiths. "Least squares ellipsoid
144  specific fitting." Geometric modeling and processing, 2004. proceedings. IEEE, 2004.
145  */
146  Eigen::MatrixXd ellipsoidLS(EigenSTL::vector_Vector3d meas);
147 
148  /*
149  This function compute magnetometer calibration parameters according to Section 5.3 of the
150  paper: Renaudin, Valérie, Muhammad Haris Afzal, and Gérard Lachapelle. "Complete triaxis
151  magnetometer calibration in the magnetic domain." Journal of sensors 2010 (2010).
152  */
153  void magCal(Eigen::MatrixXd u, Eigen::MatrixXd &A, Eigen::MatrixXd &bb);
154 };
155 
156 } // namespace rosflight
157 
158 #endif // MAVROSFLIGHT_SENSORS_MAG_H
ros::NodeHandle nh_
Definition: mag_cal.h:107
const double a23() const
Definition: mag_cal.h:96
Eigen::Vector3d intersect(Eigen::Vector3d r_m, Eigen::Vector3d r_e, Eigen::MatrixXd Q, Eigen::MatrixXd ub, double k)
Definition: mag_cal.cpp:315
message_filters::Subscriber< sensor_msgs::MagneticField > mag_subscriber_
Definition: mag_cal.h:110
CalibrateMag sensor class.
Definition: mag_cal.h:62
Eigen::MatrixXd b_
Definition: mag_cal.h:115
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
const double a21() const
Definition: mag_cal.h:94
double calibration_time_
seconds to record data for temperature compensation
Definition: mag_cal.h:121
bool calibrating_
whether a temperature calibration is in progress
Definition: mag_cal.h:119
void set_reference_magnetic_field_strength(double reference_magnetic_field)
Definition: mag_cal.cpp:132
ros::NodeHandle nh_private_
Definition: mag_cal.h:108
bool first_time_
waiting for first measurement for calibration
Definition: mag_cal.h:120
const double bz() const
Definition: mag_cal.h:102
int ransac_iters_
number of ransac iterations to fit ellipsoid to mag measurements
Definition: mag_cal.h:123
Eigen::MatrixXd A_
Definition: mag_cal.h:115
const double a31() const
Definition: mag_cal.h:97
ros::ServiceServer mag_cal_srv_
Definition: mag_cal.h:112
Eigen::MatrixXd ellipsoidRANSAC(EigenSTL::vector_Vector3d meas, int iters, double inlier_thresh)
Definition: mag_cal.cpp:198
const double a32() const
Definition: mag_cal.h:98
bool set_param(std::string name, double value)
Definition: mag_cal.cpp:506
const double a13() const
Definition: mag_cal.h:93
const double by() const
Definition: mag_cal.h:101
ros::ServiceClient param_set_client_
Definition: mag_cal.h:113
Eigen::Vector3d measurement_prev_
Definition: mag_cal.h:127
const double a33() const
Definition: mag_cal.h:99
const double a11() const
The const stuff is to make it read-only.
Definition: mag_cal.h:91
double inlier_thresh_
threshold to consider measurement an inlier in ellipsoidRANSAC
Definition: mag_cal.h:126
double start_time_
timestamp of first calibration measurement
Definition: mag_cal.h:122
void magCal(Eigen::MatrixXd u, Eigen::MatrixXd &A, Eigen::MatrixXd &bb)
Definition: mag_cal.cpp:463
Eigen::MatrixXd ellipsoidLS(EigenSTL::vector_Vector3d meas)
Definition: mag_cal.cpp:389
bool is_calibrating()
Check if a calibration is in progress.
Definition: mag_cal.h:88
void start_mag_calibration()
Begin the magnetometer calibration routine.
Definition: mag_cal.cpp:137
bool mag_callback(const sensor_msgs::MagneticField::ConstPtr &mag)
set_refence_magnetic_field_strength
Definition: mag_cal.cpp:159
EigenSTL::vector_Vector3d measurements_
Definition: mag_cal.h:128
const double a12() const
Definition: mag_cal.h:92
double reference_field_strength_
the strength of earth&#39;s magnetic field at your location
Definition: mag_cal.h:117
void eigSort(Eigen::MatrixXd &w, Eigen::MatrixXd &v)
Definition: mag_cal.cpp:338
const double bx() const
Definition: mag_cal.h:100
const double a22() const
Definition: mag_cal.h:95


rosflight
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:09:25