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

formatting

release/4.3a0
lcarlone 2021-04-03 17:37:36 -04:00
parent 038c1c0b8e
commit 71c528a87d
1 changed files with 115 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -11,7 +11,7 @@
/** /**
* @file SmartStereoProjectionFactorPP.h * @file SmartStereoProjectionFactorPP.h
* @brief Smart stereo factor on poses and extrinsic calibration * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -33,8 +33,8 @@ namespace gtsam {
*/ */
/** /**
* This factor optimizes the extrinsic camera calibration (pose of camera wrt body), * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
* and each camera has its own extrinsic calibration. * Each camera has its own extrinsic calibration.
* This factor requires that values contain the involved poses and extrinsics (both Pose3). * This factor requires that values contain the involved poses and extrinsics (both Pose3).
* @addtogroup SLAM * @addtogroup SLAM
*/ */
@ -72,9 +72,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
* @param Isotropic measurement noise * @param Isotropic measurement noise
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartStereoProjectionFactorPP( SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
const SharedNoiseModel& sharedNoiseModel, const SmartStereoProjectionParams& params =
const SmartStereoProjectionParams& params = SmartStereoProjectionParams()); SmartStereoProjectionParams());
/** Virtual destructor */ /** Virtual destructor */
~SmartStereoProjectionFactorPP() override = default; ~SmartStereoProjectionFactorPP() override = default;
@ -87,8 +87,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
* @param body_P_cam_key is key corresponding to the camera observing the same landmark * @param body_P_cam_key is key corresponding to the camera observing the same landmark
* @param K is the (fixed) camera calibration * @param K is the (fixed) camera calibration
*/ */
void add(const StereoPoint2& measured, void add(const StereoPoint2& measured, const Key& w_P_body_key,
const Key& w_P_body_key, const Key& body_P_cam_key, const Key& body_P_cam_key,
const boost::shared_ptr<Cal3_S2Stereo>& K); const boost::shared_ptr<Cal3_S2Stereo>& K);
/** /**
@ -128,7 +128,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
/// equals /// equals
const KeyVector& getExtrinsicPoseKeys() const {return body_P_cam_keys_;}; const KeyVector& getExtrinsicPoseKeys() const {
return body_P_cam_keys_;
}
;
/** /**
* error calculates the error of the factor. * error calculates the error of the factor.
@ -153,34 +156,37 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansAndCorrectForMissingMeasurements( void computeJacobiansAndCorrectForMissingMeasurements(
FBlocks& Fs, FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
Matrix& E, Vector& b, const Values& values) const {
if (!result_) { if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint"); throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians } else { // valid result: compute jacobians
size_t numViews = measured_.size(); size_t numViews = measured_.size();
E = Matrix::Zero(3*numViews,3); // a StereoPoint2 for each view E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view
b = Vector::Zero(3*numViews); // a StereoPoint2 for each view b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view
Matrix dPoseCam_dPoseBody,dPoseCam_dPoseExt, dProject_dPoseCam,Ei; Matrix dPoseCam_dPoseBody, dPoseCam_dPoseExt, dProject_dPoseCam, 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
Pose3 w_P_body = values.at<Pose3>(w_P_body_keys_.at(i)); Pose3 w_P_body = values.at<Pose3>(w_P_body_keys_.at(i));
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i)); Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
StereoCamera camera(w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt), K_all_[i]); StereoCamera camera(
StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i)); w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt),
K_all_[i]);
StereoPoint2 reprojectionError = StereoPoint2(
camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i));
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12 Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
J.block<ZDim,6>(0,0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6) J.block<ZDim, 6>(0, 0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6)
J.block<ZDim,6>(0,6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6) J.block<ZDim, 6>(0, 6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6)
if(std::isnan(measured_.at(i).uR())) // if the right pixel is invalid if (std::isnan(measured_.at(i).uR())) // if the right pixel is invalid
{ {
J.block<1,12>(1,0) = Matrix::Zero(1,12); J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
Ei.block<1,3>(1,0) = Matrix::Zero(1,3); Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() ); reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0,
reprojectionError.v());
} }
Fs.push_back(J); Fs.push_back(J);
size_t row = 3*i; size_t row = 3 * i;
b.segment<ZDim>(row) = - reprojectionError.vector(); b.segment<ZDim>(row) = -reprojectionError.vector();
E.block<3,3>(row,0) = Ei; E.block<3, 3>(row, 0) = Ei;
} }
} }
} }
@ -194,7 +200,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// 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() != cameras(values).size()) if (this->measured_.size() != cameras(values).size())
@ -205,12 +211,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
if (!result_) { if (!result_) {
// failed: return"empty" Hessian // failed: return"empty" Hessian
for(Matrix& m: Gs) for (Matrix& m : Gs)
m = Matrix::Zero(DimPose,DimPose); m = Matrix::Zero(DimPose, DimPose);
for(Vector& v: gs) for (Vector& v : gs)
v = Vector::Zero(DimPose); v = Vector::Zero(DimPose);
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, return boost::make_shared < RegularHessianFactor<DimPose>
Gs, gs, 0.0); > (keys_, Gs, gs, 0.0);
} }
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
@ -230,7 +236,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// marginalize point // marginalize point
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement<3,Dim>(Fs, E, P, b); Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
// now pack into an Hessian factor // now pack into an Hessian factor
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
@ -239,97 +245,109 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size(); size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size();
SymmetricBlockMatrix augmentedHessianUniqueKeys; SymmetricBlockMatrix augmentedHessianUniqueKeys;
if ( nrUniqueKeys == nrNonuniqueKeys ){ // if there is 1 calibration key per camera if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView())); augmentedHessianUniqueKeys = SymmetricBlockMatrix(
}else{ // if multiple cameras share a calibration dims, Matrix(augmentedHessian.selfadjointView()));
} else { // if multiple cameras share a calibration
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
nonuniqueDims.back() = 1; nonuniqueDims.back() = 1;
augmentedHessian = SymmetricBlockMatrix(nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); augmentedHessian = SymmetricBlockMatrix(
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys; KeyVector nonuniqueKeys;
for(size_t i=0; i < w_P_body_keys_.size();i++){ for (size_t i = 0; i < w_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(w_P_body_keys_.at(i)); nonuniqueKeys.push_back(w_P_body_keys_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
} }
// get map from key to location in the new augmented Hessian matrix (the one including only unique keys) // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
std::map<Key,size_t> keyToSlotMap; std::map<Key, size_t> keyToSlotMap;
for(size_t k=0; k<nrUniqueKeys;k++){ for (size_t k = 0; k < nrUniqueKeys; k++) {
keyToSlotMap[keys_[k]] = k; keyToSlotMap[keys_[k]] = k;
} }
// initialize matrix to zero // initialize matrix to zero
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1)); augmentedHessianUniqueKeys = SymmetricBlockMatrix(
dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
for(size_t i=0; i<nrNonuniqueKeys;i++){ // rows for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
Key key_i = nonuniqueKeys.at(i); Key key_i = nonuniqueKeys.at(i);
// update information vector // update information vector
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , nrUniqueKeys, augmentedHessianUniqueKeys.updateOffDiagonalBlock(
augmentedHessian.aboveDiagonalBlock(i,nrNonuniqueKeys)); keyToSlotMap[key_i], nrUniqueKeys,
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
// update blocks // update blocks
for(size_t j=i; j<nrNonuniqueKeys;j++){ // cols for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
Key key_j = nonuniqueKeys.at(j); Key key_j = nonuniqueKeys.at(j);
if(i==j){ if (i == j) {
augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i)); augmentedHessianUniqueKeys.updateDiagonalBlock(
}else{ // (i < j) keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
if( keyToSlotMap[key_i] != keyToSlotMap[key_j] ){ } else { // (i < j)
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
augmentedHessian.aboveDiagonalBlock(i,j)); augmentedHessianUniqueKeys.updateOffDiagonalBlock(
}else{ keyToSlotMap[key_i], keyToSlotMap[key_j],
augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.aboveDiagonalBlock(i, j));
augmentedHessian.aboveDiagonalBlock(i,j) + } else {
augmentedHessian.aboveDiagonalBlock(i,j).transpose()); augmentedHessianUniqueKeys.updateDiagonalBlock(
keyToSlotMap[key_i],
augmentedHessian.aboveDiagonalBlock(i, j)
+ augmentedHessian.aboveDiagonalBlock(i, j).transpose());
} }
} }
} }
} }
augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); augmentedHessianUniqueKeys.updateDiagonalBlock(
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
} }
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys); return boost::make_shared < RegularHessianFactor<DimPose>
> (keys_, augmentedHessianUniqueKeys);
} }
/** /**
* Linearize to Gaussian Factor * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor * @param values Values structure which must contain camera poses and extrinsic pose for this factor
* @return a Gaussian factor * @return a Gaussian factor
*/ */
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values, boost::shared_ptr<GaussianFactor> linearizeDamped(
const double lambda = 0.0) const { const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors // depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) { switch (params_.linearizationMode) {
case HESSIAN: case HESSIAN:
return createHessianFactor(values, lambda); return createHessianFactor(values, lambda);
default: default:
throw std::runtime_error("SmartStereoProjectionFactorPP: unknown linearization mode"); throw std::runtime_error(
"SmartStereoProjectionFactorPP: unknown linearization mode");
} }
} }
/// linearize /// linearize
boost::shared_ptr<GaussianFactor> linearize( boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
const Values& values) const override { override {
return linearizeDamped(values); return linearizeDamped(values);
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_); ar & BOOST_SERIALIZATION_NVP(K_all_);
} }
}; // end of class declaration };
// end of class declaration
/// traits /// traits
template <> template<>
struct traits<SmartStereoProjectionFactorPP> struct traits<SmartStereoProjectionFactorPP> : public Testable<
: public Testable<SmartStereoProjectionFactorPP> {}; SmartStereoProjectionFactorPP> {
};
} // namespace gtsam } // namespace gtsam