Go to the documentation of this file.
51 template <
class CAMERA>
96 const std::shared_ptr<Cameras>&
cameraRig,
101 throw std::runtime_error(
102 "SmartProjectionRigFactor: "
103 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
105 throw std::runtime_error(
106 "SmartProjectionRigFactor: "
107 "linearizationMode must be set to HESSIAN");
121 const size_t& cameraId = 0) {
124 this->nonUniqueKeys_.push_back(
poseKey);
128 if (std::find(this->
keys_.begin(),
this->keys_.end(), poseKey) ==
130 this->
keys_.push_back(poseKey);
150 throw std::runtime_error(
151 "SmartProjectionRigFactor: "
152 "trying to add inconsistent inputs");
155 throw std::runtime_error(
156 "SmartProjectionRigFactor: "
157 "camera rig includes multiple camera "
158 "but add did not input cameraIds");
182 const std::string&
s =
"",
184 std::cout <<
s <<
"SmartProjectionRigFactor: \n ";
186 std::cout <<
"-- Measurement nr " <<
i << std::endl;
188 std::cout <<
"cameraId: " <<
cameraIds_[
i] << std::endl;
189 (*cameraRig_)[
cameraIds_[
i]].print(
"camera in rig:\n");
196 const This*
e =
dynamic_cast<const This*
>(&
p);
200 e->cameraIds().begin());
214 const Pose3 world_P_sensor_i =
217 cameras.emplace_back(world_P_sensor_i,
218 make_shared<typename CAMERA::CalibrationType>(
219 camera_i.calibration()));
228 if (this->
active(values)) {
248 throw(
"computeJacobiansWithTriangulatedPoint");
251 for (
size_t i = 0;
i <
Fs.size();
i++) {
264 bool diagonalDamping =
false)
const {
268 size_t nrUniqueKeys =
275 std::vector<size_t> js;
276 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
277 std::vector<Vector> gs(nrUniqueKeys);
279 if (this->
measured_.size() != cameras.size())
280 throw std::runtime_error(
281 "SmartProjectionRigFactor: "
282 "measured_.size() inconsistent with input");
291 return std::make_shared<RegularHessianFactor<DimPose> >(this->
keys_,
294 throw std::runtime_error(
295 "SmartProjectionRigFactor: "
296 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
308 for (
size_t i = 0;
i <
Fs.size();
i++) {
312 const Matrix3
P = Base::Cameras::PointCov(
E,
lambda, diagonalDamping);
318 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
321 return std::make_shared<RegularHessianFactor<DimPose> >(
322 this->
keys_, augmentedHessianUniqueKeys);
340 throw std::runtime_error(
341 "SmartProjectionRigFactor: unknown linearization mode");
352 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
353 friend class boost::serialization::access;
355 template <
class ARCHIVE>
356 void serialize(ARCHIVE& ar,
const unsigned int ) {
357 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
367 template <
class CAMERA>
369 :
public Testable<SmartProjectionRigFactor<CAMERA> > {};
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Pose3 body_P_sensor() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CAMERA Camera
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Smart factor on cameras (pose + calibration)
Class compose(const Class &g) const
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
SmartProjectionParams params_
const KeyVector & nonUniqueKeys() const
static const SmartProjectionParams params
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
SmartProjectionRigFactor()
Default constructor, only for serialization.
virtual bool active(const Values &) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
A set of cameras, all with their own calibration.
CameraSet< CAMERA > Cameras
const std::shared_ptr< Cameras > & cameraRig() const
return the calibration object
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
CameraSet< CAMERA > Cameras
DegeneracyMode degeneracyMode
How to linearize the factor.
GenericMeasurement< Point2, Point2 > Measurement
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
LinearizationMode linearizationMode
How to linearize the factor.
TriangulationResult result_
result from triangulateSafe
SmartProjectionRigFactor< CAMERA > This
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
SharedIsotropic noiseModel_
Pose3 inverse() const
inverse transformation with derivatives
CAMERA::MeasurementVector MEASUREMENTS
CAMERA::CalibrationType CALIBRATION
FastVector< size_t > cameraIds_
noiseModel::Base::shared_ptr SharedNoiseModel
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
const FastVector< size_t > & cameraIds() const
return the calibration object
SmartProjectionFactor< CAMERA > Base
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams())
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
CAMERA::Measurement MEASUREMENT
std::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
KeyVector keys_
The keys involved in this factor.
std::vector< double > measurements
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Array< int, Dynamic, 1 > v
std::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)
CAMERA Camera
shorthand for a set of cameras
Base::Cameras cameras(const Values &values) const override
The matrix class, also used for vectors and row-vectors.
static const int ZDim
Measurement dimension.
double error(const Values &values) const override
std::uint64_t Key
Integer nonlinear key type.
static const int DimPose
Pose3 dimension.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
bool equal(const T &obj1, const T &obj2, double tol)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:31