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

98
gtsam/geometry/CameraSet.h
View File

@ -147,68 +147,74 @@ public:
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* Fixed size version
*/
template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
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) {
*/
template <int N,
int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
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) {
// a single point is observed in m cameras
size_t m = Fs.size();
// a single point is observed in m cameras
size_t m = Fs.size();
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t M1 = ND * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, ND);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// D = (Dx2) * ZDim
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setDiagonalBlock(i, FiT
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
return augmentedHessian;
}
/**
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
* G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b)
* In this version, we allow for the case where the keys in the Jacobian are organized
* differently from the keys in the output SymmetricBlockMatrix
* In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration)
* such that F keeps the block structure that makes the Schur complement trick fast.
* In this version, we allow for the case where the keys in the Jacobian are
* organized differently from the keys in the output SymmetricBlockMatrix In
* particular: each diagonal block of the Jacobian F captures 2 poses (useful
* 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(
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 KeyVector jacobianKeys, const KeyVector hessianKeys) {
const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size();

71
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

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