Loading...
Searching...
No Matches
SmartProjectionPoseFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam {
template <class CALIBRATION>
class SmartProjectionPoseFactor
: public SmartProjectionFactor<PinholePose<CALIBRATION> > {
private:
typedef PinholePose<CALIBRATION> Camera;
typedef SmartProjectionFactor<Camera> Base;
typedef SmartProjectionPoseFactor<CALIBRATION> This;
protected:
boost::shared_ptr<CALIBRATION> K_;
public:
typedef boost::shared_ptr<This> shared_ptr;
SmartProjectionPoseFactor() {}
SmartProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), K_(K) {
}
SmartProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor,
const SmartProjectionParams& params = SmartProjectionParams())
: SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
this->body_P_sensor_ = body_P_sensor;
}
~SmartProjectionPoseFactor() override {
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactor, z = \n ";
Base::print("", keyFormatter);
}
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol);
}
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
return 0.0;
}
}
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
for (const Key& k : this->keys_) {
const Pose3 world_P_sensor_k =
Base::body_P_sensor_ ? values.at<Pose3>(k) * *Base::body_P_sensor_
: values.at<Pose3>(k);
cameras.emplace_back(world_P_sensor_k, K_);
}
return cameras;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
// end of class declaration
template<class CALIBRATION>
struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartProjectionPoseFactor<CALIBRATION> > {
};
} // \ namespace gtsam
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration).
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
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
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::SmartFactorBase< PinholePose< CALIBRATION > >::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Definition SmartFactorBase.h:82
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
gtsam::SmartProjectionFactor< Camera >::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition SmartProjectionFactor.h:103
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Definition SmartProjectionFactor.h:411
gtsam::SmartProjectionFactor< Camera >::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
Definition SmartProjectionFactor.h:115
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::SmartProjectionFactor
SmartProjectionFactor()
Definition SmartProjectionFactor.h:80
gtsam::SmartProjectionPoseFactor
If you are using the factor, please cite: L.
Definition SmartProjectionPoseFactor.h:46
gtsam::SmartProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionPoseFactor.h:111
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:72
gtsam::SmartProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionPoseFactor.h:138
gtsam::SmartProjectionPoseFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionPoseFactor.h:119
gtsam::SmartProjectionPoseFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionPoseFactor.h:59
gtsam::SmartProjectionPoseFactor::K_
boost::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition SmartProjectionPoseFactor.h:54
gtsam::SmartProjectionPoseFactor::~SmartProjectionPoseFactor
~SmartProjectionPoseFactor() override
Virtual destructor.
Definition SmartProjectionPoseFactor.h:96
gtsam::SmartProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionPoseFactor.h:104
gtsam::SmartProjectionPoseFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return calibration shared pointers
Definition SmartProjectionPoseFactor.h:128
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:86
gtsam::SmartProjectionPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionPoseFactor.h:152
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor()
Default constructor, only for serialization.
Definition SmartProjectionPoseFactor.h:64