Go to the documentation of this file.
30 const Key& w_P_body_key,
const Key& body_P_cam_key,
31 const std::shared_ptr<Cal3_S2Stereo>&
K) {
39 if(std::find(
keys_.begin(),
keys_.end(), body_P_cam_key) ==
keys_.end())
40 keys_.push_back(body_P_cam_key);
48 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
50 assert(world_P_body_keys.size() == body_P_cam_keys.size());
51 assert(world_P_body_keys.size() == Ks.size());
56 keys_.push_back(body_P_cam_keys[
i]);
68 const std::shared_ptr<Cal3_S2Stereo>&
K) {
70 assert(world_P_body_keys.size() == body_P_cam_keys.size());
75 keys_.push_back(body_P_cam_keys[
i]);
85 const std::string&
s,
const KeyFormatter& keyFormatter)
const {
86 std::cout <<
s <<
"SmartStereoProjectionFactorPP: \n ";
87 for (
size_t i = 0;
i <
K_all_.size();
i++) {
88 K_all_[
i]->print(
"calibration = ");
89 std::cout <<
" extrinsic pose key: " << keyFormatter(
body_P_cam_keys_[
i]) << std::endl;
104 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 Fri Nov 1 2024 03:35:31