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

improved naming, formatting, comments

release/4.3a0
lcarlone 2021-04-03 17:45:02 -04:00
parent 71c528a87d
commit 53e3de3629
2 changed files with 37 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
View File

@ -32,7 +32,7 @@ void SmartStereoProjectionFactorPP::add(
// we index by cameras..
Base::add(measured, w_P_body_key);
// but we also store the extrinsic calibration keys in the same order
w_P_body_keys_.push_back(w_P_body_key);
world_P_body_keys_.push_back(w_P_body_key);
body_P_cam_keys_.push_back(body_P_cam_key);
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
@ -55,7 +55,7 @@ void SmartStereoProjectionFactorPP::add(
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
w_P_body_keys_.push_back(w_P_body_keys[i]);
world_P_body_keys_.push_back(w_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
K_all_.push_back(Ks[i]);
@ -74,7 +74,7 @@ void SmartStereoProjectionFactorPP::add(
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
w_P_body_keys_.push_back(w_P_body_keys[i]);
world_P_body_keys_.push_back(w_P_body_keys[i]);
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
K_all_.push_back(K);
@ -110,11 +110,11 @@ double SmartStereoProjectionFactorPP::error(const Values& values) const {
SmartStereoProjectionFactorPP::Base::Cameras
SmartStereoProjectionFactorPP::cameras(const Values& values) const {
assert(w_P_body_keys_.size() == K_all_.size());
assert(w_P_body_keys_.size() == body_P_cam_keys_.size());
assert(world_P_body_keys_.size() == K_all_.size());
assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
Base::Cameras cameras;
for (size_t i = 0; i < w_P_body_keys_.size(); i++) {
Pose3 w_P_body = values.at<Pose3>(w_P_body_keys_[i]);
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_[i]);
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));

58
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -34,8 +34,8 @@ namespace gtsam {
/**
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
* Each camera has its own extrinsic calibration.
* This factor requires that values contain the involved poses and extrinsics (both Pose3).
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras.
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables).
* @addtogroup SLAM
*/
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
@ -43,10 +43,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/// shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
/// The keys corresponding to the pose of the body for each view
KeyVector w_P_body_keys_;
/// The keys corresponding to the pose of the body (with respect to an external world frame) for each view
KeyVector world_P_body_keys_;
/// The keys corresponding to the extrinsic pose calibration for each view
/// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body)
KeyVector body_P_cam_keys_;
public:
@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
static const int Dim = 12; ///< Camera dimension
static const int DimPose = 6; ///< Camera dimension
static const int ZDim = 3; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/**
@ -82,22 +82,23 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/**
* add a new measurement, with a pose key, and an extrinsic pose key
* @param measured is the 3-dimensional location of the projection of a
* single landmark in the a single view (the measurement)
* @param w_P_body_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
* single landmark in the a single (stereo) view (the measurement)
* @param world_P_body_key is the key corresponding to the body poses observing the same landmark
* @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration
* @param K is the (fixed) camera intrinsic calibration
*/
void add(const StereoPoint2& measured, const Key& w_P_body_key,
void add(const StereoPoint2& measured, const Key& world_P_body_key,
const Key& body_P_cam_key,
const boost::shared_ptr<Cal3_S2Stereo>& K);
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the camera observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the camera observing the same landmark
* @param Ks vector of calibration objects
* @param measurements vector of the 3m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
* @param Ks vector of intrinsic calibration objects
*/
void add(const std::vector<StereoPoint2>& measurements,
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
@ -106,10 +107,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
/**
* Variant of the previous one in which we include a set of measurements with
* the same noise and calibration
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m view (the measurement)
* @param w_P_body_keys are the ordered keys corresponding to the camera observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the camera observing the same landmark
* @param measurements vector of the 3m dimensional location of the projection
* of a single landmark in the m (stereo) view (the measurements)
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(const std::vector<StereoPoint2>& measurements,
@ -131,7 +133,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
const KeyVector& getExtrinsicPoseKeys() const {
return body_P_cam_keys_;
}
;
/**
* error calculates the error of the factor.
@ -166,7 +167,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
Matrix dPoseCam_dPoseBody, dPoseCam_dPoseExt, dProject_dPoseCam, Ei;
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>(world_P_body_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),
@ -243,7 +244,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
std::fill(dims.begin(), dims.end() - 1, 6);
dims.back() = 1;
size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size();
size_t nrNonuniqueKeys = world_P_body_keys_.size()
+ body_P_cam_keys_.size();
SymmetricBlockMatrix augmentedHessianUniqueKeys;
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
@ -257,8 +259,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
KeyVector nonuniqueKeys;
for (size_t i = 0; i < w_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(w_P_body_keys_.at(i));
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
nonuniqueKeys.push_back(world_P_body_keys_.at(i));
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
}