51template <
class CAMERA>
56 typedef typename CAMERA::CalibrationType CALIBRATION;
58 static const int DimPose = 6;
59 static const int ZDim = 2;
73 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 const boost::shared_ptr<Cameras>&
cameraRig,
98 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
99 throw std::runtime_error(
100 "SmartProjectionRigFactor: "
101 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
102 if (Base::params_.linearizationMode != gtsam::HESSIAN)
103 throw std::runtime_error(
104 "SmartProjectionRigFactor: "
105 "linearizationMode must be set to HESSIAN");
122 const size_t& cameraId = 0) {
125 this->nonUniqueKeys_.push_back(poseKey);
129 if (std::find(this->
keys_.begin(), this->keys_.end(), poseKey) ==
131 this->
keys_.push_back(poseKey);
148 const FastVector<size_t>&
cameraIds = FastVector<size_t>()) {
149 if (poseKeys.size() != measurements.size() ||
151 throw std::runtime_error(
152 "SmartProjectionRigFactor: "
153 "trying to add inconsistent inputs");
156 throw std::runtime_error(
157 "SmartProjectionRigFactor: "
158 "camera rig includes multiple camera "
159 "but add did not input cameraIds");
161 for (
size_t i = 0; i < measurements.size(); i++) {
162 add(measurements[i], poseKeys[i],
183 const std::string& s =
"",
184 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
185 std::cout << s <<
"SmartProjectionRigFactor: \n ";
187 std::cout <<
"-- Measurement nr " << i << std::endl;
188 std::cout <<
"key: " << keyFormatter(
nonUniqueKeys_[i]) << std::endl;
189 std::cout <<
"cameraId: " <<
cameraIds_[i] << std::endl;
190 (*cameraRig_)[
cameraIds_[i]].print(
"camera in rig:\n");
197 const This* e =
dynamic_cast<const This*
>(&p);
201 e->cameraIds().
begin());
211 typename Base::Cameras
cameras;
215 const Pose3 world_P_sensor_i =
218 cameras.emplace_back(world_P_sensor_i,
219 make_shared<typename CAMERA::CalibrationType>(
220 camera_i.calibration()));
229 if (this->
active(values)) {
246 Matrix& E, Vector& b,
249 throw(
"computeJacobiansWithTriangulatedPoint");
252 for (
size_t i = 0; i < Fs.size(); i++) {
255 Eigen::Matrix<double, DimPose, DimPose> H;
256 world_P_body.compose(body_P_sensor, H);
257 Fs.at(i) = Fs.at(i) * H;
264 const Values& values,
const double& lambda = 0.0,
265 bool diagonalDamping =
false)
const {
269 size_t nrUniqueKeys =
276 std::vector<size_t> js;
277 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
278 std::vector<Vector> gs(nrUniqueKeys);
280 if (this->
measured_.size() != cameras.size())
281 throw std::runtime_error(
282 "SmartProjectionRigFactor: "
283 "measured_.size() inconsistent with input");
290 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
291 for (Vector& v : gs) v = Vector::Zero(DimPose);
292 return boost::make_shared<RegularHessianFactor<DimPose> >(this->
keys_,
295 throw std::runtime_error(
296 "SmartProjectionRigFactor: "
297 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
302 typename Base::FBlocks Fs;
309 for (
size_t i = 0; i < Fs.size(); i++) {
313 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
319 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
322 return boost::make_shared<RegularHessianFactor<DimPose> >(
323 this->
keys_, augmentedHessianUniqueKeys);
334 const Values& values,
const double& lambda = 0.0)
const {
341 throw std::runtime_error(
342 "SmartProjectionRigFactor: unknown linearization mode");
348 const Values& values)
const override {
355 template <
class ARCHIVE>
356 void serialize(ARCHIVE& ar,
const unsigned int ) {
357 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
366template <
class CAMERA>
368 :
public Testable<SmartProjectionRigFactor<CAMERA> > {};
Smart factor on cameras (pose + calibration)
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:84
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
Definition: SymmetricBlockMatrix.h:52
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:50
This is the base class for all factor types.
Definition: Factor.h:56
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:73
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:128
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:346
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition: SmartFactorBase.h:72
ZVector measured_
Measurements for each of the m views.
Definition: SmartFactorBase.h:79
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:159
Definition: SmartFactorParams.h:42
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition: SmartProjectionFactor.h:45
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionFactor.h:103
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:63
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:174
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition: SmartProjectionFactor.h:411
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:115
CAMERA Camera
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:74
Definition: SmartProjectionRigFactor.h:52
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition: SmartProjectionRigFactor.h:63
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition: SmartProjectionRigFactor.h:70
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionRigFactor.h:175
boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition: SmartProjectionRigFactor.h:263
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:79
void add(const Point2Vector &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Variant of the previous "add" function in which we include multiple measurements.
Definition: SmartProjectionRigFactor.h:147
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute jacobian F, E and error vector at a given linearization point.
Definition: SmartProjectionRigFactor.h:245
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition: SmartProjectionRigFactor.h:82
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartProjectionRigFactor.h:228
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartProjectionRigFactor.h:210
~SmartProjectionRigFactor() override=default
Virtual destructor.
void add(const Point2 &measured, const Key &poseKey, const size_t &cameraId=0)
add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera ...
Definition: SmartProjectionRigFactor.h:121
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionRigFactor.h:182
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionRigFactor.h:347
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionRigFactor.h:354
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition: SmartProjectionRigFactor.h:169
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition: SmartProjectionRigFactor.h:66
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams())
Constructor.
Definition: SmartProjectionRigFactor.h:92
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionRigFactor.h:172
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
Definition: SmartProjectionRigFactor.h:333
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionRigFactor.h:196