Go to the documentation of this file.
23 #include <gtsam_unstable/dllexport.h>
44 template <
class CAMERA>
81 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
102 const std::shared_ptr<Cameras>&
cameraRig,
107 throw std::runtime_error(
108 "SmartProjectionRigFactor: "
109 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
111 throw std::runtime_error(
112 "SmartProjectionRigFactor: "
113 "linearizationMode must be set to HESSIAN");
129 const Key& world_P_body_key2,
const double&
alpha,
130 const size_t& cameraId = 0) {
136 std::make_pair(world_P_body_key1, world_P_body_key2));
140 if (std::find(this->
keys_.begin(),
this->keys_.end(), world_P_body_key1) ==
142 this->
keys_.push_back(world_P_body_key1);
143 if (std::find(this->
keys_.begin(),
this->keys_.end(), world_P_body_key2) ==
145 this->
keys_.push_back(world_P_body_key2);
169 const std::vector<double>&
alphas,
175 throw std::runtime_error(
176 "SmartProjectionPoseFactorRollingShutter: "
177 "trying to add inconsistent inputs");
180 throw std::runtime_error(
181 "SmartProjectionPoseFactorRollingShutter: "
182 "camera rig includes multiple camera "
183 "but add did not input cameraIds");
215 const std::string&
s =
"",
217 std::cout <<
s <<
"SmartProjectionPoseFactorRollingShutter: \n ";
219 std::cout <<
"-- Measurement nr " <<
i << std::endl;
220 std::cout <<
" pose1 key: "
222 std::cout <<
" pose2 key: "
224 std::cout <<
" alpha: " <<
alphas_[
i] << std::endl;
225 std::cout <<
"cameraId: " <<
cameraIds_[
i] << std::endl;
226 (*cameraRig_)[
cameraIds_[
i]].print(
"camera in rig:\n");
237 double keyPairsEqual =
true;
238 if (this->world_P_body_key_pairs_.size() ==
239 e->world_P_body_key_pairs().size()) {
240 for (
size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
242 const Key key1e =
e->world_P_body_key_pairs()[k].first;
244 const Key key2e =
e->world_P_body_key_pairs()[k].second;
245 if (!(key1own == key1e) || !(key2own == key2e)) {
246 keyPairsEqual =
false;
251 keyPairsEqual =
false;
255 keyPairsEqual &&
cameraRig_->equals(*(
e->cameraRig())) &&
257 e->cameraIds().begin());
270 const Pose3& w_P_body1 =
272 const Pose3& w_P_body2 =
274 double interpolationFactor =
alphas_[
i];
275 const Pose3& w_P_body =
276 interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
278 const Pose3& body_P_cam = camera_i.pose();
281 make_shared<typename CAMERA::CalibrationType>(
282 camera_i.calibration()));
291 if (this->
active(values)) {
310 throw(
"computeJacobiansWithTriangulatedPoint");
312 size_t numViews = this->
measured_.size();
313 E = Matrix::Zero(2 * numViews,
315 b = Vector::Zero(2 * numViews);
319 dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
322 for (
size_t i = 0;
i < numViews;
i++) {
325 double interpolationFactor =
alphas_[
i];
328 interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
329 dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
331 auto body_P_cam = camera_i.pose();
332 auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
334 w_P_cam, make_shared<typename CAMERA::CalibrationType>(
335 camera_i.calibration()));
341 J.block(0, 0,
ZDim, 6) =
342 dProject_dPoseCam * dPoseCam_dInterpPose *
343 dInterpPose_dPoseBody1;
344 J.block(0, 6,
ZDim, 6) =
345 dProject_dPoseCam * dPoseCam_dInterpPose *
346 dInterpPose_dPoseBody2;
351 b.segment<
ZDim>(
row) = -reprojectionError_i;
360 bool diagonalDamping =
false)
const {
364 size_t nrUniqueKeys =
372 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
373 std::vector<Vector> gs(nrUniqueKeys);
377 throw std::runtime_error(
378 "SmartProjectionPoseFactorRollingShutter: "
379 "measured_.size() inconsistent with input");
388 return std::make_shared<RegularHessianFactor<DimPose>>(this->
keys_,
391 throw std::runtime_error(
392 "SmartProjectionPoseFactorRollingShutter: "
393 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
404 for (
size_t i = 0;
i <
Fs.size();
i++)
421 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
424 return std::make_shared<RegularHessianFactor<DimPose>>(
425 this->
keys_, augmentedHessianUniqueKeys);
443 throw std::runtime_error(
444 "SmartProjectionPoseFactorRollingShutter: "
445 "unknown linearization mode");
456 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
457 friend class boost::serialization::access;
459 template <
class ARCHIVE>
460 void serialize(ARCHIVE& ar,
const unsigned int ) {
461 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
468 template <
class CAMERA>
470 :
public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
CameraSet< CAMERA > Cameras
Base::Cameras cameras(const Values &values) const override
const std::shared_ptr< Cameras > & cameraRig() const
return the calibration object
CAMERA::MeasurementVector MEASUREMENTS
Base class to create smart factors on poses or cameras.
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::vector< std::pair< Key, Key > > world_P_body_key_pairs_
const FastVector< size_t > & cameraIds() const
return the calibration object
double error(const Values &values) const override
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
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).
SmartProjectionPoseFactorRollingShutter< CAMERA > This
Eigen::Matrix< double, ZDim, DimBlock > MatrixZD
SmartProjectionParams params_
SmartProjectionPoseFactorRollingShutter()
Default constructor, only for serialization.
std::shared_ptr< typename Base::Cameras > cameraRig_
static const int ZDim
Measurement dimension (Point2)
static const SmartProjectionParams params
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
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.
A set of cameras, all with their own calibration.
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
CameraSet< CAMERA > Cameras
DegeneracyMode degeneracyMode
How to linearize the factor.
GenericMeasurement< Point2, Point2 > Measurement
CAMERA::Measurement MEASUREMENT
LinearizationMode linearizationMode
How to linearize the factor.
JacobiRotation< float > J
TriangulationResult result_
result from triangulateSafe
SmartProjectionFactor< CAMERA > Base
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
const std::vector< std::pair< Key, Key > > & world_P_body_key_pairs() const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
SharedIsotropic noiseModel_
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
SmartProjectionPoseFactorRollingShutter(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams ¶ms=SmartProjectionParams())
const std::vector< double > & alphas() const
return the interpolation factors alphas
void add(const MEASUREMENTS &measurements, const std::vector< std::pair< Key, Key >> &world_P_body_key_pairs, const std::vector< double > &alphas, const FastVector< size_t > &cameraIds=FastVector< size_t >())
noiseModel::Base::shared_ptr SharedNoiseModel
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
KeyVector keys_
The keys involved in this factor.
std::vector< double > measurements
CAMERA::CalibrationType CALIBRATION
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
static const int DimPose
Pose3 dimension.
Array< int, Dynamic, 1 > v
CAMERA Camera
shorthand for a set of cameras
static const EIGEN_MAKE_ALIGNED_OPERATOR_NEW int DimBlock
std::vector< double > alphas_
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)
The matrix class, also used for vectors and row-vectors.
FastVector< size_t > cameraIds_
static const CalibratedCamera camera(kDefaultPose)
std::uint64_t Key
Integer nonlinear key type.
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
void add(const MEASUREMENT &measured, const Key &world_P_body_key1, const Key &world_P_body_key2, const double &alpha, const size_t &cameraId=0)
bool equal(const T &obj1, const T &obj2, double tol)
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:30