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

now I need to move to testing and interpolation

release/4.3a0
lcarlone 2021-07-19 17:01:27 -04:00
parent 1e2a1d2591
commit 16d624d4e1
1 changed files with 97 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -67,9 +67,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
static const int Dim = 6; ///< Pose3 dimension static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2) static const int ZDim = 2; ///< Measurement dimension (Point2)
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera) typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/** /**
@ -247,96 +248,101 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
* respect to both the body pose and extrinsic pose), the point Jacobian E, * respect to both the body pose and extrinsic pose), the point Jacobian E,
* and the error vector b. Note that the jacobians are computed for a given point. * and the error vector b. Note that the jacobians are computed for a given point.
*/ */
// void computeJacobiansAndCorrectForMissingMeasurements( void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { const Values& values) const
// if (!result_) { override {
// throw("computeJacobiansWithTriangulatedPoint"); if (!this->result_) {
// } else { // valid result: compute jacobians throw("computeJacobiansWithTriangulatedPoint");
// size_t numViews = measured_.size(); } else { // valid result: compute jacobians
// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) size_t numViews = this->measured_.size();
// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian)
// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; b = Vector::Zero(2 * numViews); // a Point2 for each view
// Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
// Pose3 w_P_body = values.at<Pose3>(world_P_body_key_pairs_.at(i)); dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
// Pose3 body_P_cam = values.at<Pose3>(body_P_cam_ this->keys_.at(i)); Eigen::Matrix<double, ZDim, 3> Ei;
// StereoCamera camera(
// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
// K_all_[i]); Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
// // get jacobians and error vector for current measurement Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
// StereoPoint2 reprojectionError_i = StereoPoint2( double interpolationFactor = gammas_[i];
// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); // get interpolated pose:
// Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12 std::cout << "TODO: need to add proper interpolation and Jacobians here" << std::endl;
// J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2,
// J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) interpolationFactor); /*dInterpPose_dPoseBody1, dInterpPose_dPoseBody2 */
// // if the right pixel is invalid, fix jacobians Pose3 body_P_cam = body_P_sensors_[i];
// if (std::isnan(measured_.at(i).uR())) Pose3 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
// { PinholeCamera<CALIBRATION> camera(w_P_cam, K_all_[i]);
// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); // get jacobians and error vector for current measurement
// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, Point2 reprojectionError_i = Point2(
// reprojectionError_i.v()); camera.project(*this->result_, dProject_dPoseCam, Ei)
// } - this->measured_.at(i));
// // fit into the output structures Eigen::Matrix<double, ZDim, DimBlock> J; // 2 x 12
// Fs.push_back(J); J.block<ZDim, 6>(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose
// size_t row = 3 * i; * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
// b.segment<ZDim>(row) = -reprojectionError_i.vector(); J.block<ZDim, 6>(0, 6) = dProject_dPoseCam * dPoseCam_dInterpPose
// E.block<3, 3>(row, 0) = Ei; * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6)
// }
// } // fit into the output structures
// } Fs.push_back(J);
size_t row = 2 * i;
b.segment<ZDim>(row) = -reprojectionError_i;
E.block<3, 3>(row, 0) = Ei;
}
}
}
/// 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<DimBlock> > createHessianFactor(
// const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double lambda = 0.0, bool diagonalDamping =
// false) const { false) const override {
//
// // we may have multiple cameras sharing the same extrinsic cals, hence the number // we may have multiple cameras sharing the same extrinsic cals, hence the number
// // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
// // have a body key and an extrinsic calibration key for each measurement) // have a body key and an extrinsic calibration key for each measurement)
// size_t nrUniqueKeys = this->keys_.size(); size_t nrUniqueKeys = this->keys_.size();
// size_t nrNonuniqueKeys = world_P_body_key_pairs_.size() size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size();
// + body_P_cam_ this->keys_.size();
// // 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()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
// throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" "measured_.size() inconsistent with input");
// "measured_.size() inconsistent with input");
// // triangulate 3D point at given linearization point
// // triangulate 3D point at given linearization point triangulateSafe(cameras(values));
// triangulateSafe(cameras(values));
// if (!this->result_) { // failed: return "empty/zero" Hessian
// if (!result_) { // failed: return "empty/zero" 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>
// return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
// > ( this->keys_, Gs, gs, 0.0); }
// }
// // compute Jacobian given triangulated 3D Point
// // compute Jacobian given triangulated 3D Point FBlocks Fs;
// FBlocks Fs; Matrix F, E;
// Matrix F, E; Vector b;
// Vector b; computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
// // Whiten using noise model
// // Whiten using noise model this->noiseModel_->WhitenSystem(E, b);
// noiseModel_->WhitenSystem(E, b); for (size_t i = 0; i < Fs.size(); i++)
// for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]);
// Fs[i] = noiseModel_->Whiten(Fs[i]);
//
// // build augmented Hessian (with last row/column being the information vector) // // build augmented Hessian (with last row/column being the information vector)
// Matrix3 P; // Matrix3 P;
// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
// //
// // marginalize point: note - we reuse the standard SchurComplement function // // marginalize point: note - we reuse the standard SchurComplement function
// SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(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
// std::fill(dims.begin(), dims.end() - 1, 6); // std::fill(dims.begin(), dims.end() - 1, 6);
@ -408,7 +414,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
// } // }
// return boost::make_shared < RegularHessianFactor<DimPose> // return boost::make_shared < RegularHessianFactor<DimPose>
// > ( this->keys_, augmentedHessianUniqueKeys); // > ( this->keys_, augmentedHessianUniqueKeys);
// } }
/** /**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * 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
@ -418,8 +425,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
const Values& values, 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 (this->params_.linearizationMode) { switch (this->params_.linearizationMode) {
// case HESSIAN: case HESSIAN:
// return createHessianFactor(values, lambda); return createHessianFactor(values, lambda);
default: default:
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); "SmartProjectionPoseFactorRollingShutter: unknown linearization mode");