SmartStereoProjectionPoseFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 
24 #include <cassert>
25 
26 namespace gtsam {
27 
29  const SharedNoiseModel& sharedNoiseModel,
31  const std::optional<Pose3>& body_P_sensor)
32  : Base(sharedNoiseModel, params, body_P_sensor) {}
33 
35  const StereoPoint2& measured, const Key& poseKey,
36  const std::shared_ptr<Cal3_S2Stereo>& K) {
38  K_all_.push_back(K);
39 }
40 
42  const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
43  const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
44  assert(measurements.size() == poseKeys.size());
45  assert(poseKeys.size() == Ks.size());
46  Base::add(measurements, poseKeys);
47  K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
48 }
49 
51  const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
52  const std::shared_ptr<Cal3_S2Stereo>& K) {
53  assert(poseKeys.size() == measurements.size());
54  for (size_t i = 0; i < measurements.size(); i++) {
55  Base::add(measurements[i], poseKeys[i]);
56  K_all_.push_back(K);
57  }
58 }
59 
61  const std::string& s, const KeyFormatter& keyFormatter) const {
62  std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
63  for (const std::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
64  K->print("calibration = ");
65  }
66  Base::print("", keyFormatter);
67 }
68 
70  double tol) const {
72  dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
73 
74  return e && Base::equals(p, tol);
75 }
76 
78  if (this->active(values)) {
79  return this->totalReprojectionError(cameras(values));
80  } else { // else of active flag
81  return 0.0;
82  }
83 }
84 
87  assert(keys_.size() == K_all_.size());
89  for (size_t i = 0; i < keys_.size(); i++) {
90  Pose3 pose = values.at<Pose3>(keys_[i]);
91  if (Base::body_P_sensor_) {
92  pose = pose.compose(*(Base::body_P_sensor_));
93  }
94  cameras.push_back(StereoCamera(pose, K_all_[i]));
95  }
96  return cameras;
97 }
98 
99 } // \ namespace gtsam
SmartStereoProjectionPoseFactor.h
Smart stereo factor on poses, assuming camera calibration is fixed.
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::SmartStereoProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartStereoProjectionFactor.h:420
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartFactorBase< StereoCamera >::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:164
gtsam::SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor
SmartStereoProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams &params=SmartStereoProjectionParams(), const std::optional< Pose3 > &body_P_sensor={})
Definition: SmartStereoProjectionPoseFactor.cpp:28
measured
Point2 measured(-17, 30)
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:142
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::SmartStereoProjectionPoseFactor
Definition: SmartStereoProjectionPoseFactor.h:46
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::Pose3
Definition: Pose3.h:37
add
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
gtsam::SmartStereoProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartStereoProjectionPoseFactor.cpp:69
vanillaPose::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:76
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::SmartStereoProjectionPoseFactor::add
void add(const StereoPoint2 &measured, const Key &poseKey, const std::shared_ptr< Cal3_S2Stereo > &K)
Definition: SmartStereoProjectionPoseFactor.cpp:34
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::SmartStereoProjectionPoseFactor::error
double error(const Values &values) const override
Definition: SmartStereoProjectionPoseFactor.cpp:77
K
#define K
Definition: igam.h:8
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::StereoCamera
Definition: StereoCamera.h:47
gtsam::SmartStereoProjectionPoseFactor::K_all_
std::vector< std::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
Definition: SmartStereoProjectionPoseFactor.h:50
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
gtsam::SmartStereoProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartStereoProjectionPoseFactor.cpp:86
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SmartStereoProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartStereoProjectionPoseFactor.cpp:60
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:28