Go to the documentation of this file.
12 #include <gtsam_unstable/dllexport.h>
68 throw std::invalid_argument(
69 "SmartRangeFactor::addRange: adding duplicate measurement for key.");
81 void print(
const std::string&
s =
"",
83 std::cout <<
s <<
"SmartRangeFactor with " <<
size() <<
" measurements\n";
101 std::list<Circle2> circles;
103 for (
size_t j = 0;
j <
n;
j++) {
108 Circle2 circle1 = circles.front();
109 std::optional<Point2> best_fh;
110 std::optional<Circle2> bestCircle2 = std::nullopt;
113 for (
const Circle2& it : circles) {
123 if (fh && (!best_fh || fh->y() > best_fh->y())) {
130 if (bestCircle2 && best_fh) {
131 auto bestCircleCenter = bestCircle2->center;
132 std::list<Point2> intersections =
136 double error1 = 0, error2 = 0;
137 Point2 p1 = intersections.front(),
p2 = intersections.back();
138 for (
const Circle2& it : circles) {
142 return (error1 < error2) ?
p1 :
p2;
144 throw std::runtime_error(
"triangulate failed");
156 for (
size_t j = 0;
j <
n;
j++)
157 (*
H)[
j] = Matrix::Zero(3, 1);
169 for (
size_t j = 0;
j <
n;
j++) {
183 return std::static_pointer_cast<gtsam::NonlinearFactor>(
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
std::shared_ptr< This > shared_ptr
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
double error(const Values &c) const override
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
~SmartRangeFactor() override
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
SharedNoiseModel noiseModel_
void addRange(Key key, double measuredRange)
Add a range measurement to a pose with given key.
Point2 triangulate(const Values &x) const
gtsam::NonlinearFactor::shared_ptr clone() const override
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Circle2(const Point2 &p, double r)
const SharedNoiseModel & noiseModel() const
access to the noise model
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
double variance_
variance on noise
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
std::vector< Matrix > * OptionalMatrixVecType
std::vector< double > measurements_
Range measurements.
KeyVector keys_
The keys involved in this factor.
std::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
std::uint64_t Key
Integer nonlinear key type.
virtual Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const =0
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
SmartRangeFactor(double s)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:23