12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some formatting

release/4.3a0
Frank Dellaert 2021-08-28 17:19:19 -04:00
parent f712d62150
commit 0c622294d2
2 changed files with 93 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/geometry/CameraSet.h
View File

@ -148,11 +148,13 @@ public:
* g = F' * (b - E * P * E' * b) * g = F' * (b - E * P * E' * b)
* Fixed size version * Fixed size version
*/ */
template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension template <int N,
int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
static SymmetricBlockMatrix SchurComplement( static SymmetricBlockMatrix SchurComplement(
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs, 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) { const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
// a single point is observed in m cameras // a single point is observed in m cameras
size_t m = Fs.size(); size_t m = Fs.size();
@ -197,18 +199,22 @@ public:
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F * G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b) * g = F' * (b - E * P * E' * b)
* In this version, we allow for the case where the keys in the Jacobian are organized * In this version, we allow for the case where the keys in the Jacobian are
* differently from the keys in the output SymmetricBlockMatrix * organized differently from the keys in the output SymmetricBlockMatrix In
* In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) * particular: each diagonal block of the Jacobian F captures 2 poses (useful
* such that F keeps the block structure that makes the Schur complement trick fast. * for rolling shutter and extrinsic calibration) such that F keeps the block
* structure that makes the Schur complement trick fast.
*
* N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
* the Hessian block dimension
*/ */
template<int N, int ND, int NDD> // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension template <int N, int ND, int NDD>
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
const std::vector<Eigen::Matrix<double, ZDim, ND>, const std::vector<
Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND> > >& Fs, 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, const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
const KeyVector jacobianKeys, const KeyVector hessianKeys) { const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size(); size_t nrUniqueKeys = hessianKeys.size();

67
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/geometry/CameraSet.h>
namespace gtsam { namespace gtsam {
/** /**
@ -229,7 +230,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
keyPairsEqual = false; break; keyPairsEqual = false; break;
} }
} }
}else{ keyPairsEqual = false; } } else {
keyPairsEqual = false;
}
double extrinsicCalibrationEqual = true; double extrinsicCalibrationEqual = true;
if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ if(this->body_P_sensors_.size() == e->body_P_sensors().size()){
@ -238,7 +241,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
extrinsicCalibrationEqual = false; break; extrinsicCalibrationEqual = false; break;
} }
} }
}else{ extrinsicCalibrationEqual = false; } } else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration() return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
@ -248,9 +253,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* Compute jacobian F, E and error vector at a given linearization point * Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses * @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor * corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with * @return Return arguments are the camera jacobians Fs (including the
* respect to both body poses we interpolate from), the point Jacobian E, * jacobian with respect to both body poses we interpolate from), the point
* and the error vector b. Note that the jacobians are computed for a given point. * Jacobian E, and the error vector b. Note that the jacobians are computed
* for a given point.
*/ */
void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
@ -267,13 +273,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
Eigen::Matrix<double, ZDim, 3> Ei; Eigen::Matrix<double, ZDim, 3> Ei;
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i]; double interpolationFactor = alphas_[i];
// get interpolated pose: // get interpolated pose:
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); auto w_P_body =
const Pose3& body_P_cam = body_P_sensors_[i]; interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = body_P_sensors_[i];
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]); PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
@ -296,21 +304,25 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double lambda = 0.0,
false) const { bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (due to the
// we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // rolling shutter interpolation), hence the number of unique keys may be
// hence the number of unique keys may be smaller than 2 * nrMeasurements // smaller than 2 * nrMeasurements
size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector < Vector > gs(nrUniqueKeys); std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera if (this->measured_.size() !=
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " this->cameras(values).size()) // 1 observation per interpolated camera
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input"); "measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point // triangulate 3D point at given linearization point
@ -318,14 +330,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
if (!this->result_) { // failed: return "empty/zero" Hessian if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs) for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose);
for (Vector& v : gs) return boost::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
v = Vector::Zero(DimPose); Gs, gs, 0.0);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0);
} else { } else {
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"only supported degeneracy mode is ZERO_ON_DEGENERACY"); "only supported degeneracy mode is ZERO_ON_DEGENERACY");
} }
} }