37 : Base(model, key1, key2), measured_(measured) {}
46 std::cout << s <<
"PoseToPointFactor(" << keyFormatter(this->
key1()) <<
"," 47 << keyFormatter(this->
key2()) <<
")\n" 48 <<
" measured: " << measured_.transpose() << std::endl;
54 double tol = 1
e-9)
const {
55 const This*
e =
dynamic_cast<const This*
>(&
expected);
70 boost::optional<Matrix&> H1 = boost::none,
71 boost::optional<Matrix&> H2 = boost::none)
const {
81 template <
class ARCHIVE>
83 ar& boost::serialization::make_nvp(
84 "NoiseModelFactor2", boost::serialization::base_object<Base>(*
this));
85 ar& BOOST_SERIALIZATION_NVP(measured_);
boost::shared_ptr< PoseToPointFactor > shared_ptr
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Diagonal::shared_ptr model
virtual bool equals(const NonlinearFactor &expected, double tol=1e-9) const
NoiseModelFactor2< Pose3, Point3 > Base
void serialize(ARCHIVE &ar, const unsigned int)
static const KeyFormatter DefaultKeyFormatter
virtual void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Vector evaluateError(const Pose3 &wTwi, const Point3 &wPwp, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Error = wTwi.inverse()*wPwp - measured_.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
friend class boost::serialization::access
SharedNoiseModel noiseModel_
const Point3 & measured() const
Non-linear factor base classes.
virtual ~PoseToPointFactor()
PoseToPointFactor(Key key1, Key key2, const Point3 &measured, const SharedNoiseModel &model)
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel