SmartStereoProjectionFactorPP.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
22 
23 namespace gtsam {
43 class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
45  protected:
47  std::vector<std::shared_ptr<Cal3_S2Stereo>> K_all_;
48 
51 
54 
55  public:
57 
60 
63 
65  typedef std::shared_ptr<This> shared_ptr;
66 
67  static const int DimBlock = 12;
68  static const int DimPose = 6;
69  static const int ZDim = 3;
70  typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
71  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
72 
78  SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
81 
90  void add(const StereoPoint2& measured, const Key& world_P_body_key,
91  const Key& body_P_cam_key,
92  const std::shared_ptr<Cal3_S2Stereo>& K);
93 
103  void add(const std::vector<StereoPoint2>& measurements,
104  const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
105  const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks);
106 
117  void add(const std::vector<StereoPoint2>& measurements,
118  const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
119  const std::shared_ptr<Cal3_S2Stereo>& K);
120 
126  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
127  DefaultKeyFormatter) const override;
128 
130  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
131 
134  return body_P_cam_keys_;
135  }
136 
140  double error(const Values& values) const override;
141 
143  inline std::vector<std::shared_ptr<Cal3_S2Stereo>> calibration() const {
144  return K_all_;
145  }
146 
154  Base::Cameras cameras(const Values& values) const override;
155 
165  FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
166  if (!result_) {
167  throw("computeJacobiansWithTriangulatedPoint");
168  } else { // valid result: compute jacobians
169  size_t numViews = measured_.size();
170  E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian)
171  b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view
172  Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
173 
174  for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
175  Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_.at(i));
176  Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
178  w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i),
179  K_all_[i]);
180  // get jacobians and error vector for current measurement
181  StereoPoint2 reprojectionError_i = StereoPoint2(
182  camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i));
184  J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
185  J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
186  // if the right pixel is invalid, fix jacobians
187  if (std::isnan(measured_.at(i).uR()))
188  {
189  J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
190  Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
191  reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0,
192  reprojectionError_i.v());
193  }
194  // fit into the output structures
195  Fs.push_back(J);
196  size_t row = 3 * i;
197  b.segment<ZDim>(row) = -reprojectionError_i.vector();
198  E.block<3, 3>(row, 0) = Ei;
199  }
200  }
201  }
202 
204  std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
205  const Values& values, const double lambda = 0.0,
206  bool diagonalDamping = false) const {
207  // we may have multiple cameras sharing the same extrinsic cals, hence the number
208  // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
209  // have a body key and an extrinsic calibration key for each measurement)
210  size_t nrUniqueKeys = keys_.size();
211 
212  // Create structures for Hessian Factors
213  KeyVector js;
214  std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
215  std::vector<Vector> gs(nrUniqueKeys);
216 
217  if (this->measured_.size() != cameras(values).size())
218  throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
219  "measured_.size() inconsistent with input");
220 
221  // triangulate 3D point at given linearization point
222  triangulateSafe(cameras(values));
223 
224  // failed: return "empty/zero" Hessian
225  if (!result_) {
226  for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
227  for (Vector& v : gs) v = Vector::Zero(DimPose);
228  return std::make_shared<RegularHessianFactor<DimPose>>(keys_, Gs, gs,
229  0.0);
230  }
231 
232  // compute Jacobian given triangulated 3D Point
233  FBlocks Fs;
234  Matrix F, E;
235  Vector b;
236  computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
237 
238  // Whiten using noise model
239  noiseModel_->WhitenSystem(E, b);
240  for (size_t i = 0; i < Fs.size(); i++) {
241  Fs[i] = noiseModel_->Whiten(Fs[i]);
242  }
243 
244  // build augmented Hessian (with last row/column being the information vector)
245  Matrix3 P;
246  Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
247 
248  // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
249  KeyVector nonuniqueKeys;
250  for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
251  nonuniqueKeys.push_back(world_P_body_keys_.at(i));
252  nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
253  }
254  // but we need to get the augumented hessian wrt the unique keys in key_
255  SymmetricBlockMatrix augmentedHessianUniqueKeys =
256  Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock,
257  DimPose>(
258  Fs, E, P, b, nonuniqueKeys, keys_);
259 
260  return std::make_shared<RegularHessianFactor<DimPose>>(
261  keys_, augmentedHessianUniqueKeys);
262  }
263 
269  std::shared_ptr<GaussianFactor> linearizeDamped(
270  const Values& values, const double lambda = 0.0) const {
271  // depending on flag set on construction we may linearize to different linear factors
272  switch (params_.linearizationMode) {
273  case HESSIAN:
274  return createHessianFactor(values, lambda);
275  default:
276  throw std::runtime_error(
277  "SmartStereoProjectionFactorPP: unknown linearization mode");
278  }
279  }
280 
282  std::shared_ptr<GaussianFactor> linearize(const Values& values) const
283  override {
284  return linearizeDamped(values);
285  }
286 
287  private:
288 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
289  friend class boost::serialization::access;
291  template<class ARCHIVE>
292  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
293  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
294  ar & BOOST_SERIALIZATION_NVP(K_all_);
295  }
296 #endif
297 };
298 // end of class declaration
299 
301 template<>
303  SmartStereoProjectionFactorPP> {
304 };
305 
306 } // namespace gtsam
gtsam::SmartStereoProjectionFactorPP::body_P_cam_keys_
KeyVector body_P_cam_keys_
The keys corresponding to the extrinsic pose calibration for each view (pose that transform from came...
Definition: SmartStereoProjectionFactorPP.h:53
gtsam::StereoPoint2::uL
double uL() const
get uL
Definition: StereoPoint2.h:110
gtsam::StereoPoint2::vector
Vector3 vector() const
Definition: StereoPoint2.h:119
gtsam::add
static Y add(const Y &y1, const Y &y2)
Definition: HybridGaussianProductFactor.cpp:32
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartStereoProjectionFactorPP::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartStereoProjectionFactorPP.h:65
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::SmartStereoProjectionFactorPP::createHessianFactor
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)
Definition: SmartStereoProjectionFactorPP.h:204
gtsam::SmartStereoProjectionFactorPP::computeJacobiansAndCorrectForMissingMeasurements
void computeJacobiansAndCorrectForMissingMeasurements(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Definition: SmartStereoProjectionFactorPP.h:164
gtsam::StereoPoint2::v
double v() const
get v
Definition: StereoPoint2.h:116
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
isnan
#define isnan(X)
Definition: main.h:93
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::SmartStereoProjectionFactorPP::K_all_
std::vector< std::shared_ptr< Cal3_S2Stereo > > K_all_
shared pointer to calibration object (one for each camera)
Definition: SmartStereoProjectionFactorPP.h:47
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::SmartStereoProjectionParams
SmartProjectionParams SmartStereoProjectionParams
Definition: SmartStereoProjectionFactor.h:45
gtsam::SmartStereoProjectionFactorPP::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Definition: SmartStereoProjectionFactorPP.h:269
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::SmartStereoProjectionFactorPP::getExtrinsicPoseKeys
const KeyVector & getExtrinsicPoseKeys() const
equals
Definition: SmartStereoProjectionFactorPP.h:133
gtsam::SmartStereoProjectionFactorPP::calibration
std::vector< std::shared_ptr< Cal3_S2Stereo > > calibration() const
Definition: SmartStereoProjectionFactorPP.h:143
DimBlock
static const int DimBlock
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:216
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Definition: base/Matrix.h:221
gtsam::SmartStereoProjectionFactor
Definition: SmartStereoProjectionFactor.h:55
gtsam::KeyFormatter
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
gtsam::Pose3
Definition: Pose3.h:37
ZDim
static const int ZDim
Measurement dimension (Point2)
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:218
gtsam::SmartStereoProjectionFactorPP::Base
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef SmartStereoProjectionFactor Base
shorthand for base class type
Definition: SmartStereoProjectionFactorPP.h:59
gtsam::SmartStereoProjectionFactorPP::This
SmartStereoProjectionFactorPP This
shorthand for this class
Definition: SmartStereoProjectionFactorPP.h:62
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:698
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
vanillaPose::Cameras
CameraSet< Camera > Cameras
Definition: smartFactorScenarios.h:76
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
gtsam::SmartStereoProjectionFactorPP::world_P_body_keys_
KeyVector world_P_body_keys_
The keys corresponding to the pose of the body (with respect to an external world frame) for each vie...
Definition: SmartStereoProjectionFactorPP.h:50
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
lambda
static double lambda[]
Definition: jv.c:524
gtsam::equals
Definition: Testable.h:112
E
DiscreteKey E(5, 2)
gtsam::b
const G & b
Definition: Group.h:79
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
SmartStereoProjectionFactor.h
Smart stereo factor on StereoCameras (pose)
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Values
Definition: Values.h:65
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::SmartStereoProjectionFactorPP::MatrixZD
Eigen::Matrix< double, ZDim, DimBlock > MatrixZD
Definition: SmartStereoProjectionFactorPP.h:70
gtsam::StereoCamera
Definition: StereoCamera.h:47
P
static double P[]
Definition: ellpe.c:68
gtsam::tol
const G double tol
Definition: Group.h:79
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
camera
static const CalibratedCamera camera(kDefaultPose)
Base
Definition: test_virtual_functions.cpp:156
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::SmartStereoProjectionFactorPP::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartStereoProjectionFactorPP.h:71
gtsam::SmartStereoProjectionFactorPP
Definition: SmartStereoProjectionFactorPP.h:43
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartStereoProjectionFactorPP::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartStereoProjectionFactorPP.h:282


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:31