Go to the documentation of this file.
49 using NoiseModelFactor1<VALUE>::evaluateError;
89 std::placeholders::_1, std::placeholders::_2, 1
e-9)) :
100 std::placeholders::_1, std::placeholders::_2, 1
e-9)) :
112 std::cout << (
s.empty() ?
s :
s +
" ") <<
"Constraint: on ["
113 << keyFormatter(this->
key()) <<
"]\n";
121 const This*
e =
dynamic_cast<const This*
>(&
f);
132 const T& xj =
c.at<
T>(this->
key());
146 *
H = Matrix::Identity(nj,nj);
150 *
H = Matrix::Identity(nj,nj);
151 return Vector::Zero(nj);
154 throw std::invalid_argument(
155 "Linearization point not feasible for "
157 return Vector::Constant(nj, std::numeric_limits<double>::infinity());
163 const T& xj =
x.at<
T>(this->
key());
173 return std::static_pointer_cast<gtsam::NonlinearFactor>(
183 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
184 friend class boost::serialization::access;
186 template<
class ARCHIVE>
187 void serialize(ARCHIVE & ar,
const unsigned int ) {
190 & boost::serialization::make_nvp(
"NoiseModelFactor1",
191 boost::serialization::base_object<Base>(*
this));
201 template <
typename VALUE>
208 template<
class VALUE>
215 using NoiseModelFactor1<VALUE>::evaluateError;
232 typedef std::shared_ptr<NonlinearEquality1<VALUE> >
shared_ptr;
251 return std::static_pointer_cast<gtsam::NonlinearFactor>(
266 std::cout <<
s <<
": NonlinearEquality1(" << keyFormatter(this->
key())
276 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
277 friend class boost::serialization::access;
279 template<
class ARCHIVE>
280 void serialize(ARCHIVE & ar,
const unsigned int ) {
283 & boost::serialization::make_nvp(
"NoiseModelFactor1",
284 boost::serialization::base_object<Base>(*
this));
285 ar & BOOST_SERIALIZATION_NVP(
value_);
291 template <
typename VALUE>
293 :
Testable<NonlinearEquality1<VALUE> > {};
331 return std::static_pointer_cast<gtsam::NonlinearFactor>(
339 if (H1) *H1 = -Matrix::Identity(
p,
p);
340 if (H2) *H2 = Matrix::Identity(
p,
p);
347 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
348 friend class boost::serialization::access;
350 template <
class ARCHIVE>
351 void serialize(ARCHIVE& ar,
const unsigned int ) {
353 ar& boost::serialization::make_nvp(
354 "NoiseModelFactor2", boost::serialization::base_object<Base>(*
this));
360 template <
typename VALUE>
NonlinearEquality()
Default constructor - only for serialization.
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
NonlinearEquality2< T > This
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Concept check for values that can be used in unit tests.
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
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
gtsam::NonlinearFactor::shared_ptr clone() const override
gtsam::NonlinearFactor::shared_ptr clone() const override
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
SharedNoiseModel noiseModel_
std::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
NonlinearEquality1< VALUE > This
NoiseModelFactorN< VALUE > Base
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.
virtual Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const =0
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double error(const Values &c) const override
Actual error function calculation.
noiseModel::Diagonal::shared_ptr SharedDiagonal
double dot(const V1 &a, const V2 &b)
static shared_ptr All(size_t dim)
Base class and basic functions for Manifold types.
~NonlinearEquality2() override
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Diagonal::shared_ptr model
Vector evaluateError(const T &x1, const T &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
g(x) with optional derivative2
Vector evaluateError(const T &xj, OptionalMatrixType H) const override
Error function.
Vector evaluateError(const X &x1, OptionalMatrixType H) const override
g(x) with optional derivative
std::shared_ptr< NonlinearEquality2< T > > shared_ptr
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
~NonlinearEquality1() override
~NonlinearEquality() override
NonlinearEquality1()
Default constructor to allow for serialization.
gtsam::NonlinearFactor::shared_ptr clone() const override
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
NonlinearEquality< VALUE > This
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Matrix * OptionalMatrixType
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
std::uint64_t Key
Integer nonlinear key type.
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50