12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/gtsam_unstable/slam/SmartStereoProjectionFactor.h

526 lines
18 KiB
C++
Raw Blame History

/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SmartStereoProjectionFactor.h
* @brief Smart stereo factor on StereoCameras (pose)
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
*/
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam_unstable/dllexport.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/optional.hpp>
#endif
#include <optional>
#include <vector>
namespace gtsam {
/*
* Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
*/
typedef SmartProjectionParams SmartStereoProjectionParams;
/**
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with StereoCamera. This factor requires that values
* contains the involved StereoCameras. Calibration is assumed to be fixed, as this
* is also assumed in StereoCamera.
* If you'd like to store poses in values instead of cameras, use
* SmartStereoProjectionPoseFactor instead
*/
class SmartStereoProjectionFactor
: public SmartFactorBase<StereoCamera> {
private:
typedef SmartFactorBase<StereoCamera> Base;
protected:
/// @name Parameters
/// @{
const SmartStereoProjectionParams params_;
/// @}
/// @name Caching triangulation
/// @{
mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
/// @}
public:
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
/// Vector of cameras
typedef CameraSet<StereoCamera> Cameras;
/// Vector of monocular cameras (stereo treated as 2 monocular)
typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras;
typedef MonoCamera::MeasurementVector MonoMeasurements;
/**
* Constructor
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const std::optional<Pose3> body_P_sensor = {}) :
Base(sharedNoiseModel, body_P_sensor), //
params_(params), //
result_(TriangulationResult::Degenerate()) {
}
/** Virtual destructor */
~SmartStereoProjectionFactor() override {
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartStereoProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const SmartStereoProjectionFactor *e =
dynamic_cast<const SmartStereoProjectionFactor*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol);
}
/// Check if the new linearization point_ is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
size_t m = cameras.size();
bool retriangulate = false;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true;
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
}
}
if (retriangulate) { // we store the current poses used for triangulation
cameraPosesTriangulation_.clear();
cameraPosesTriangulation_.reserve(m);
for (size_t i = 0; i < m; i++)
// cameraPosesTriangulation_[i] = cameras[i].pose();
cameraPosesTriangulation_.push_back(cameras[i].pose());
}
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
// /// triangulateSafe
// size_t triangulateSafe(const Values& values) const {
// return triangulateSafe(this->cameras(values));
// }
/// triangulateSafe
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras);
// triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
MonoCameras monoCameras;
MonoMeasurements monoMeasured;
for(size_t i = 0; i < m; i++) {
const Pose3 leftPose = cameras[i].pose();
const Cal3_S2 monoCal = cameras[i].calibration().calibration();
const MonoCamera leftCamera_i(leftPose,monoCal);
const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
const Pose3 rightPose = leftPose.compose( left_Pose_right );
const MonoCamera rightCamera_i(rightPose,monoCal);
const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v()));
if(!std::isnan(zi.uR())){ // if right point is valid
monoCameras.push_back(rightCamera_i);
monoMeasured.push_back(Point2(zi.uR(),zi.v()));
}
}
if (retriangulate)
result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
params_.triangulation);
return result_;
}
/// triangulate
bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_
return bool(result_);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
std::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
false) const {
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size())
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
"measured_.size() inconsistent with input");
triangulateSafe(cameras);
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
for(Matrix& m: Gs)
m = Matrix::Zero(Base::Dim, Base::Dim);
for(Vector& v: gs)
v = Vector::Zero(Base::Dim);
return std::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
Base::FBlocks Fs;
Matrix F, E;
Vector b;
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model
Base::whitenJacobians(Fs, E, b);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
return std::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
augmentedHessian);
}
// create factor
// std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
// else
// // failed: return empty
// return std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
// }
//
// /// create factor
// std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createJacobianQFactor(cameras, *result_, lambda);
// else
// // failed: return empty
// return std::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
// }
//
// /// Create a factor, takes values
// std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Values& values, double lambda) const {
// return createJacobianQFactor(this->cameras(values), lambda);
// }
/// different (faster) way to compute Jacobian factor
std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
return std::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
}
// /// linearize to a Hessianfactor
// virtual std::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
// const Values& values, double lambda = 0.0) const {
// return createHessianFactor(this->cameras(values), lambda);
// }
// /// linearize to an Implicit Schur factor
// virtual std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
// const Values& values, double lambda = 0.0) const {
// return createRegularImplicitSchurFactor(this->cameras(values), lambda);
// }
//
// /// linearize to a JacobianfactorQ
// virtual std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
// const Values& values, double lambda = 0.0) const {
// return createJacobianQFactor(this->cameras(values), lambda);
// }
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
std::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
// case IMPLICIT_SCHUR:
// return createRegularImplicitSchurFactor(cameras, lambda);
case JACOBIAN_SVD:
return createJacobianSVDFactor(cameras, lambda);
// case JACOBIAN_Q:
// return createJacobianQFactor(cameras, lambda);
default:
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
}
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
std::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
Cameras cameras = this->cameras(values);
return linearizeDamped(cameras, lambda);
}
/// linearize
std::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return linearizeDamped(values);
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(*result_, nullptr, &E);
return nonDegenerate;
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateAndComputeE(E, cameras);
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint(
FBlocks& Fs,
Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint");
// // Handle degeneracy
// // TODO check flag whether we should do this
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// this->measured_.at(0)); */
//
// Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else {
// valid result: just return Base version
Base::computeJacobians(Fs, E, b, cameras, *result_);
}
}
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(
FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool triangulateAndComputeJacobiansSVD(
FBlocks& Fs, Matrix& Enull, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return Vector::Zero(cameras.size() * Base::ZDim);
}
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
double totalReprojectionError(const Cameras& cameras,
std::optional<Point3> externalPoint = {}) const {
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
// // Otherwise, manage the exceptions with rotation-only factors
// const StereoPoint2& z0 = this->measured_.at(0);
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
//
// return Base::totalReprojectionError(cameras, backprojected);
} else {
// if we don't want to manage the exceptions we discard the factor
return 0.0;
}
}
/// Calculate total reprojection error
double error(const Values& values) const override {
if (this->active(values)) {
return totalReprojectionError(Base::cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* This corrects the Jacobians and error vector for the case in which the
* right 2D measurement in the monocular camera is missing (nan).
*/
void correctForMissingMeasurements(
const Cameras& cameras, Vector& ue,
typename Cameras::FBlocks* Fs = nullptr,
Matrix* E = nullptr) const override {
// when using stereo cameras, some of the measurements might be missing:
for (size_t i = 0; i < cameras.size(); i++) {
const StereoPoint2& z = measured_.at(i);
if (std::isnan(z.uR())) // if the right 2D measurement is invalid
{
if (Fs) { // delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i);
for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
}
if (E) // delete influence of right point on jacobian E
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero
ue(ZDim * i + 1) = 0.0;
}
}
}
/** return the landmark */
TriangulationResult point() const {
return result_;
}
/** COMPUTE the landmark */
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
/// Is result valid?
bool isValid() const { return result_.valid(); }
/** return the degenerate state */
bool isDegenerate() const { return result_.degenerate(); }
/** return the cheirality status flag */
bool isPointBehindCamera() const { return result_.behindCamera(); }
/** return the outlier state */
bool isOutlier() const { return result_.outlier(); }
/** return the farPoint state */
bool isFarPoint() const { return result_.farPoint(); }
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
}
#endif
};
/// traits
template<>
struct traits<SmartStereoProjectionFactor > : public Testable<
SmartStereoProjectionFactor> {
};
} // \ namespace gtsam
Reference in New Issue View Git Blame Copy Permalink