31 #include <gtsam_unstable/dllexport.h> 33 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 34 #include <boost/serialization/optional.hpp> 65 const SmartStereoProjectionParams
params_;
77 typedef std::shared_ptr<SmartStereoProjectionFactor>
shared_ptr;
110 std::cout <<
s <<
"SmartStereoProjectionFactor\n";
112 std::cout <<
"triangulationParameters:\n" << params_.
triangulation << std::endl;
113 std::cout <<
"result:\n" << result_ << std::endl;
131 size_t m = cameras.size();
133 bool retriangulate =
false;
136 if (cameraPosesTriangulation_.empty()
137 || cameras.size() != cameraPosesTriangulation_.size())
138 retriangulate =
true;
140 if (!retriangulate) {
141 for (
size_t i = 0;
i < cameras.size();
i++) {
142 if (!cameras[
i].
pose().equals(cameraPosesTriangulation_[
i],
144 retriangulate =
true;
151 cameraPosesTriangulation_.clear();
152 cameraPosesTriangulation_.reserve(m);
153 for (
size_t i = 0;
i <
m;
i++)
155 cameraPosesTriangulation_.push_back(cameras[
i].pose());
158 return retriangulate;
169 size_t m = cameras.size();
173 MonoCameras monoCameras;
174 MonoMeasurements monoMeasured;
175 for(
size_t i = 0;
i <
m;
i++) {
176 const Pose3 leftPose = cameras[
i].pose();
177 const Cal3_S2 monoCal = cameras[
i].calibration().calibration();
178 const MonoCamera leftCamera_i(leftPose,monoCal);
180 const Pose3 rightPose = leftPose.
compose( left_Pose_right );
181 const MonoCamera rightCamera_i(rightPose,monoCal);
183 monoCameras.push_back(leftCamera_i);
184 monoMeasured.push_back(
Point2(zi.
uL(),zi.
v()));
186 monoCameras.push_back(rightCamera_i);
187 monoMeasured.push_back(
Point2(zi.
uR(),zi.
v()));
199 return bool(result_);
204 const Cameras&
cameras,
const double lambda = 0.0,
bool diagonalDamping =
207 size_t numKeys = this->
keys_.size();
210 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
211 std::vector<Vector> gs(numKeys);
213 if (this->
measured_.size() != cameras.size())
214 throw std::runtime_error(
"SmartStereoProjectionHessianFactor: this->" 215 "measured_.size() inconsistent with input");
225 return std::make_shared<RegularHessianFactor<Base::Dim> >(this->
keys_,
242 return std::make_shared<RegularHessianFactor<Base::Dim> >(this->
keys_,
278 return std::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->
keys_);
305 const double lambda = 0.0)
const {
317 throw std::runtime_error(
"SmartStereoFactorlinearize: unknown mode");
327 const double lambda = 0.0)
const {
346 cameras.
project2(*result_,
nullptr, &E);
347 return nonDegenerate;
366 const Cameras&
cameras)
const {
369 throw (
"computeJacobiansWithTriangulatedPoint");
390 return nonDegenerate;
401 return nonDegenerate;
411 return Vector::Zero(cameras.size() *
Base::ZDim);
421 std::optional<Point3> externalPoint = {})
const {
432 throw(std::runtime_error(
"Backproject at infinity not implemented for SmartStereo."));
446 if (this->
active(values)) {
458 const Cameras& cameras,
Vector& ue,
460 Matrix*
E =
nullptr)
const override {
462 for (
size_t i = 0;
i < cameras.size();
i++) {
468 for (
size_t ii = 0; ii <
Dim; ii++) Fi(1, ii) = 0.0;
471 E->row(
ZDim *
i + 1) = Matrix::Zero(1,
E->cols());
474 ue(
ZDim *
i + 1) = 0.0;
486 Cameras cameras = this->
cameras(values);
507 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 508 friend class boost::serialization::access;
510 template<
class ARCHIVE>
511 void serialize(ARCHIVE & ar,
const unsigned int ) {
512 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
522 SmartStereoProjectionFactor> {
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
MonoCamera::MeasurementVector MonoMeasurements
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
bool triangulateForLinearize(const Cameras &cameras) const
triangulate
PinholeCamera< Cal3_S2 > MonoCamera
Vector of monocular cameras (stereo treated as 2 monocular)
Pose3 body_P_sensor() const
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::string serialize(const T &input)
serializes to a string
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
DegeneracyMode degeneracyMode
How to linearize the factor.
std::vector< Pose3 > cameraPosesTriangulation_
current triangulation poses
bool isDegenerate() const
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
TriangulationParameters triangulation
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
KeyVector keys_
The keys involved in this factor.
double error(const Values &values) const override
Calculate total reprojection error.
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
different (faster) way to compute Jacobian factor
SmartFactorBase< StereoCamera > Base
SmartStereoProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartStereoProjectionParams ¶ms=SmartStereoProjectionParams(), const std::optional< Pose3 > body_P_sensor={})
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
bool isValid() const
Is result valid?
A set of cameras, all with their own calibration.
bool triangulateAndComputeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
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)
static const KeyFormatter DefaultKeyFormatter
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static const SmartProjectionParams params
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point_ is the same as the one used for previous triangulation.
std::shared_ptr< SmartStereoProjectionFactor > shared_ptr
shorthand for a smart pointer to a factor
TriangulationResult point() const
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Base class to create smart factors on poses or cameras.
ZVector project2(const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
~SmartStereoProjectionFactor() override
Base class for smart factors. This base class has no internal point, but it has a measurement...
const SmartStereoProjectionParams params_
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
CameraSet< StereoCamera > Cameras
Vector of cameras.
bool triangulateAndComputeE(Matrix &E, const Values &values) const
std::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Functions for triangulation.
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
bool isPointBehindCamera() const
LinearizationMode linearizationMode
How to linearize the factor.
TriangulationResult point(const Values &values) const
The most common 5DOF 3D->2D calibration.
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms)
triangulateSafe: extensive checking of the outcome
CameraSet< MonoCamera > MonoCameras
Class compose(const Class &g) const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
A non-linear factor for stereo measurements.
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
TriangulationResult triangulateSafe(const Cameras &cameras) const
triangulateSafe
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const override
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
A Stereo Camera based on two Simple Cameras.
virtual bool active(const Values &) const
static const int Dim
Camera dimension.
static TriangulationResult Degenerate()
TriangulationResult result_
result from triangulateSafe
Point2Vector MeasurementVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
The matrix class, also used for vectors and row-vectors.
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
utility functions for loading datasets
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
std::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
linearize returns a Hessianfactor that is an approximation of error(p)
bool triangulateAndComputeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
static const int ZDim
Measurement dimension.
Collect common parameters for SmartProjection and SmartStereoProjection factors.
noiseModel::Base::shared_ptr SharedNoiseModel
SmartProjectionParams SmartStereoProjectionParams
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
bool behindCamera() const
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
double retriangulationThreshold
threshold to decide whether to re-triangulate