44template <
class CALIBRATION>
54 boost::shared_ptr<CALIBRATION>
K_;
74 const boost::shared_ptr<CALIBRATION> K,
76 : Base(sharedNoiseModel, params),
K_(K) {
88 const boost::shared_ptr<CALIBRATION> K,
89 const boost::optional<Pose3> body_P_sensor,
105 DefaultKeyFormatter)
const override {
106 std::cout << s <<
"SmartProjectionPoseFactor, z = \n ";
107 Base::print(
"", keyFormatter);
112 const This *e =
dynamic_cast<const This*
>(&p);
113 return e && Base::equals(p, tol);
120 if (this->
active(values)) {
139 typename Base::Cameras
cameras;
141 const Pose3 world_P_sensor_k =
142 Base::body_P_sensor_ ? values.
at<
Pose3>(k) * *Base::body_P_sensor_
144 cameras.emplace_back(world_P_sensor_k,
K_);
153 template<
class ARCHIVE>
154 void serialize(ARCHIVE & ar,
const unsigned int ) {
155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
156 ar & BOOST_SERIALIZATION_NVP(
K_);
162template<
class CALIBRATION>
164 SmartProjectionPoseFactor<CALIBRATION> > {
Smart factor on cameras (pose + calibration)
Global functions in a separate testing namespace.
Definition chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
Nonlinear factor base class.
Definition NonlinearFactor.h:42
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
Definition SmartFactorParams.h:42
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
If you are using the factor, please cite: L.
Definition SmartProjectionPoseFactor.h:46
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionPoseFactor.h:111
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams ¶ms=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:72
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionPoseFactor.h:138
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionPoseFactor.h:119
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionPoseFactor.h:59
boost::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition SmartProjectionPoseFactor.h:54
~SmartProjectionPoseFactor() override
Virtual destructor.
Definition SmartProjectionPoseFactor.h:96
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionPoseFactor.h:104
const boost::shared_ptr< CALIBRATION > calibration() const
return calibration shared pointers
Definition SmartProjectionPoseFactor.h:128
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams ¶ms=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:86
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionPoseFactor.h:152
SmartProjectionPoseFactor()
Default constructor, only for serialization.
Definition SmartProjectionPoseFactor.h:64