Go to the documentation of this file.
43 template<
class CAMERA>
63 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
89 :
Base(sharedNoiseModel),
104 std::cout <<
s <<
"SmartProjectionFactor\n";
109 std::cout <<
"result:\n" <<
result_ << std::endl;
115 const This *
e =
dynamic_cast<const This*
>(&
p);
136 bool retriangulate =
false;
142 retriangulate =
true;
145 if (!retriangulate) {
149 retriangulate =
true;
159 for (
size_t i = 0;
i <
m;
i++)
164 return retriangulate;
200 bool diagonalDamping =
false)
const {
201 size_t numKeys = this->
keys_.size();
204 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
205 std::vector<Vector> gs(numKeys);
207 if (this->
measured_.size() != cameras.size())
208 throw std::runtime_error(
209 "SmartProjectionHessianFactor: this->measured_"
210 ".size() inconsistent with input");
218 return std::make_shared<RegularHessianFactor<Base::Dim> >(this->
keys_,
235 return std::make_shared<RegularHessianFactor<Base::Dim> >(
236 this->
keys_, augmentedHessian);
246 return std::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
256 return std::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->
keys_);
272 return std::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->
keys_);
299 const double lambda = 0.0)
const {
311 throw std::runtime_error(
"SmartFactorlinearize: unknown mode");
321 const double lambda = 0.0)
const {
342 return nonDegenerate;
364 Unit3 backProjected =
cameras[0].backprojectPointAtInfinity(
381 return nonDegenerate;
392 return nonDegenerate;
402 return Vector::Zero(
cameras.size() * 2);
412 std::optional<Point3> externalPoint = {})
const {
424 Unit3 backprojected =
cameras.front().backprojectPointAtInfinity(
434 if (this->
active(values)) {
469 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
470 friend class boost::serialization::access;
472 template<
class ARCHIVE>
473 void serialize(ARCHIVE & ar,
const unsigned int version) {
474 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
475 ar & BOOST_SERIALIZATION_NVP(
params_);
476 ar & BOOST_SERIALIZATION_NVP(
result_);
484 template<
class CAMERA>
486 SmartProjectionFactor<CAMERA> > {
TriangulationParameters triangulation
double error(const Values &values) const override
Calculate total reprojection error.
Base class to create smart factors on poses or cameras.
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Collect common parameters for SmartProjection and SmartStereoProjection factors.
bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
ZVector project2(const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
SmartProjectionFactor< CAMERA > SmartProjectionCameraFactor
SmartProjectionParams params_
static const int Dim
Camera dimension.
TriangulationResult point(const Values &values) const
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
bool isDegenerate() const
static const SmartProjectionParams params
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
virtual bool active(const Values &) const
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Functions for triangulation.
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
A set of cameras, all with their own calibration.
bool isValid() const
Is result valid?
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
CameraSet< CAMERA > Cameras
DegeneracyMode degeneracyMode
How to linearize the factor.
LinearizationMode linearizationMode
How to linearize the factor.
virtual std::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
virtual std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
TriangulationResult result_
result from triangulateSafe
utility functions for loading datasets
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND >>> &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
virtual std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
std::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
std::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms)
triangulateSafe: extensive checking of the outcome
std::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
Create a Hessianfactor that is an approximation of error(p).
noiseModel::Base::shared_ptr SharedNoiseModel
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
static TriangulationResult Degenerate()
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Base class for smart factors. This base class has no internal point, but it has a measurement,...
~SmartProjectionFactor() override
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point is the same as the one used for previous triangulation.
KeyVector keys_
The keys involved in this factor.
bool isPointBehindCamera() const
bool behindCamera() const
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams ¶ms=SmartProjectionParams())
Array< int, Dynamic, 1 > v
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
double retriangulationThreshold
threshold to decide whether to re-triangulate
CAMERA Camera
shorthand for a set of cameras
SmartProjectionFactor< CAMERA > This
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Represents a 3D point on a unit sphere.
std::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
TriangulationResult point() const
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, double lambda) const
SmartFactorBase< CAMERA > Base
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:30