51template <
class CAMERA>
56 typedef typename CAMERA::CalibrationType CALIBRATION;
57 typedef typename CAMERA::Measurement MEASUREMENT;
58 typedef typename CAMERA::MeasurementVector MEASUREMENTS;
60 static const int DimPose = 6;
61 static const int ZDim = 2;
75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
96 const boost::shared_ptr<Cameras>&
cameraRig,
100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
101 throw std::runtime_error(
102 "SmartProjectionRigFactor: "
103 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
104 if (Base::params_.linearizationMode != gtsam::HESSIAN)
105 throw std::runtime_error(
106 "SmartProjectionRigFactor: "
107 "linearizationMode must be set to HESSIAN");
124 const size_t& cameraId = 0) {
127 this->nonUniqueKeys_.push_back(poseKey);
131 if (std::find(this->
keys_.begin(), this->keys_.end(), poseKey) ==
133 this->
keys_.push_back(poseKey);
151 if (poseKeys.size() != measurements.size() ||
153 throw std::runtime_error(
154 "SmartProjectionRigFactor: "
155 "trying to add inconsistent inputs");
158 throw std::runtime_error(
159 "SmartProjectionRigFactor: "
160 "camera rig includes multiple camera "
161 "but add did not input cameraIds");
163 for (
size_t i = 0; i < measurements.size(); i++) {
164 add(measurements[i], poseKeys[i],
185 const std::string& s =
"",
186 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
187 std::cout << s <<
"SmartProjectionRigFactor: \n ";
189 std::cout <<
"-- Measurement nr " << i << std::endl;
190 std::cout <<
"key: " << keyFormatter(
nonUniqueKeys_[i]) << std::endl;
191 std::cout <<
"cameraId: " <<
cameraIds_[i] << std::endl;
192 (*cameraRig_)[
cameraIds_[i]].print(
"camera in rig:\n");
199 const This* e =
dynamic_cast<const This*
>(&p);
203 e->cameraIds().
begin());
213 typename Base::Cameras
cameras;
217 const Pose3 world_P_sensor_i =
220 cameras.emplace_back(world_P_sensor_i,
221 make_shared<typename CAMERA::CalibrationType>(
222 camera_i.calibration()));
231 if (this->
active(values)) {
248 Matrix& E, Vector& b,
251 throw(
"computeJacobiansWithTriangulatedPoint");
254 for (
size_t i = 0; i < Fs.size(); i++) {
257 Eigen::Matrix<double, DimPose, DimPose> H;
258 world_P_body.compose(body_P_sensor, H);
259 Fs.at(i) = Fs.at(i) * H;
266 const Values& values,
const double& lambda = 0.0,
267 bool diagonalDamping =
false)
const {
271 size_t nrUniqueKeys =
278 std::vector<size_t> js;
279 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
280 std::vector<Vector> gs(nrUniqueKeys);
282 if (this->
measured_.size() != cameras.size())
283 throw std::runtime_error(
284 "SmartProjectionRigFactor: "
285 "measured_.size() inconsistent with input");
292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
293 for (Vector& v : gs) v = Vector::Zero(DimPose);
294 return boost::make_shared<RegularHessianFactor<DimPose> >(this->
keys_,
297 throw std::runtime_error(
298 "SmartProjectionRigFactor: "
299 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
304 typename Base::FBlocks Fs;
311 for (
size_t i = 0; i < Fs.size(); i++) {
315 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
324 return boost::make_shared<RegularHessianFactor<DimPose> >(
325 this->
keys_, augmentedHessianUniqueKeys);
336 const Values& values,
const double& lambda = 0.0)
const {
343 throw std::runtime_error(
344 "SmartProjectionRigFactor: unknown linearization mode");
350 const Values& values)
const override {
357 template <
class ARCHIVE>
358 void serialize(ARCHIVE& ar,
const unsigned int ) {
359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
368template <
class CAMERA>
370 :
public Testable<SmartProjectionRigFactor<CAMERA> > {};
Smart factor on cameras (pose + calibration)
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition: FastVector.h:34
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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
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:100
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
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
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:36
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:85
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:143
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:118
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:361
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
If you are using the factor, please cite: L.
Definition: SmartProjectionRigFactor.h:52
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition: SmartProjectionRigFactor.h:65
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition: SmartProjectionRigFactor.h:72
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionRigFactor.h:177
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:265
void add(const MEASUREMENT &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:123
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:81
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:247
void add(const MEASUREMENTS &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:149
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition: SmartProjectionRigFactor.h:84
double error(const Values &values) const override
error calculates the error of the factor.
Definition: SmartProjectionRigFactor.h:230
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition: SmartProjectionRigFactor.h:212
~SmartProjectionRigFactor() override=default
Virtual destructor.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartProjectionRigFactor.h:184
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionRigFactor.h:349
friend class boost::serialization::access
Serialization function.
Definition: SmartProjectionRigFactor.h:356
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition: SmartProjectionRigFactor.h:171
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition: SmartProjectionRigFactor.h:68
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams())
Constructor.
Definition: SmartProjectionRigFactor.h:94
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionRigFactor.h:174
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:335
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionRigFactor.h:198