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

Merge branch 'feature/cameraTemplateForAllSmartFactors' into feature/sphericalCamera 

# Conflicts:
#	gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
release/4.3a0
lcarlone 2021-11-07 15:20:16 -05:00
parent 2f57a1a307 c105aa4e1e
commit 78a8b7dc0e
3 changed files with 66 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -73,6 +73,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
FastVector<size_t> cameraIds_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
@ -129,7 +131,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
}
/** Virtual destructor */
~SmartProjectionRigFactor() override {}
~SmartProjectionRigFactor() override = default;
/**
* add a new measurement, corresponding to an observation from pose "poseKey"
@ -178,8 +180,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"SmartProjectionRigFactor: "
"camera rig includes multiple camera but add did not input "
"cameraIds");
"camera rig includes multiple camera "
"but add did not input cameraIds");
}
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], poseKeys[i],
@ -378,9 +380,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
ar& BOOST_SERIALIZATION_NVP(cameraRig_);
ar& BOOST_SERIALIZATION_NVP(cameraIds_);
//ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
// ar& BOOST_SERIALIZATION_NVP(cameraRig_);
//ar& BOOST_SERIALIZATION_NVP(cameraIds_);
}
};
// end of class declaration

8
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -1225,16 +1225,16 @@ TEST(SmartProjectionRigFactor, timing) {
size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) {
SmartRigFactor::shared_ptr smartFactorP(
SmartRigFactor::shared_ptr smartRigFactor(
new SmartRigFactor(model, cameraRig, params));
smartFactorP->add(measurements_lmk1[0], x1, cameraId1);
smartFactorP->add(measurements_lmk1[1], x1, cameraId1);
smartRigFactor->add(measurements_lmk1[0], x1, cameraId1);
smartRigFactor->add(measurements_lmk1[1], x1, cameraId1);
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
gttic_(SmartRigFactor_LINEARIZE);
smartFactorP->linearize(values);
smartRigFactor->linearize(values);
gttoc_(SmartRigFactor_LINEARIZE);
}

103
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -50,10 +50,6 @@ class SmartProjectionPoseFactorRollingShutter
typedef typename CAMERA::Measurement MEASUREMENT;
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
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)
protected:
/// The keys of the pose of the body (with respect to an external world
/// frame): two consecutive poses for each observation
@ -72,12 +68,20 @@ class SmartProjectionPoseFactorRollingShutter
FastVector<size_t> cameraIds_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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)
typedef Eigen::Matrix<double, ZDim, DimBlock>
MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
FBlocks; // vector of F blocks
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -198,8 +202,8 @@ class SmartProjectionPoseFactorRollingShutter
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"camera rig includes multiple camera but add did not input "
"cameraIds");
"camera rig includes multiple camera "
"but add did not input cameraIds");
}
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
@ -276,6 +280,44 @@ class SmartProjectionPoseFactorRollingShutter
e->cameraIds().begin());
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
for (size_t i = 0; i < this->measured_.size();
i++) { // for each 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);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
const Pose3& body_P_cam = camera_i.pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam,
make_shared<typename CAMERA::CalibrationType>(
camera_i.calibration()));
}
return cameras;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
@ -406,43 +448,6 @@ class SmartProjectionPoseFactorRollingShutter
this->keys_, augmentedHessianUniqueKeys);
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
for (size_t i = 0; i < this->measured_.size();
i++) { // for each 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);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
}
return cameras;
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* LM)
@ -459,8 +464,8 @@ class SmartProjectionPoseFactorRollingShutter
return this->createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: unknown linearization "
"mode");
"SmartProjectionPoseFactorRollingShutter: "
"unknown linearization mode");
}
}