Go to the documentation of this file.
32 const Key& w_P_body_key,
const Key& body_P_cam_key,
33 const std::shared_ptr<Cal3_S2Stereo>&
K) {
41 if(std::find(
keys_.begin(),
keys_.end(), body_P_cam_key) ==
keys_.end())
42 keys_.push_back(body_P_cam_key);
50 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
52 assert(world_P_body_keys.size() == body_P_cam_keys.size());
53 assert(world_P_body_keys.size() == Ks.size());
58 keys_.push_back(body_P_cam_keys[
i]);
70 const std::shared_ptr<Cal3_S2Stereo>&
K) {
72 assert(world_P_body_keys.size() == body_P_cam_keys.size());
77 keys_.push_back(body_P_cam_keys[
i]);
87 const std::string&
s,
const KeyFormatter& keyFormatter)
const {
88 std::cout <<
s <<
"SmartStereoProjectionFactorPP: \n ";
89 for (
size_t i = 0;
i <
K_all_.size();
i++) {
90 K_all_[
i]->print(
"calibration = ");
91 std::cout <<
" extrinsic pose key: " << keyFormatter(
body_P_cam_keys_[
i]) << std::endl;
106 if (this->
active(values)) {
KeyVector body_P_cam_keys_
The keys corresponding to the extrinsic pose calibration for each view (pose that transform from came...
Base::Cameras cameras(const Values &values) const override
Smart stereo factor on poses and extrinsic calibration.
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Class compose(const Class &g) const
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
void add(const StereoPoint2 &measured, const Key &world_P_body_key, const Key &body_P_cam_key, const std::shared_ptr< Cal3_S2Stereo > &K)
static const SmartProjectionParams params
std::vector< std::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
virtual bool active(const Values &) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
double error(const Values &values) const override
CameraSet< Camera > Cameras
noiseModel::Base::shared_ptr SharedNoiseModel
KeyVector world_P_body_keys_
The keys corresponding to the pose of the body (with respect to an external world frame) for each vie...
KeyVector keys_
The keys involved in this factor.
std::vector< double > measurements
bool equals(const This &other, double tol=1e-9) const
check equality
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
SmartStereoProjectionFactorPP(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams ¶ms=SmartStereoProjectionParams())
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
std::uint64_t Key
Integer nonlinear key type.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:28