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

final cosmetics

release/4.3a0
lcarlone 2021-11-07 14:50:56 -05:00
parent 2e8d373ff5
commit b1baf6c8b3
3 changed files with 56 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -71,6 +71,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
FastVector<size_t> cameraIds_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
@ -127,7 +129,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
}
/** Virtual destructor */
~SmartProjectionRigFactor() override {}
~SmartProjectionRigFactor() override = default;
/**
* add a new measurement, corresponding to an observation from pose "poseKey"
@ -176,8 +178,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],
@ -376,9 +378,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

@ -1229,16 +1229,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);
}

85
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -84,6 +84,9 @@ class SmartProjectionPoseFactorRollingShutter
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
FBlocks; // vector of F blocks
/// Default constructor, only for serialization
SmartProjectionPoseFactorRollingShutter() {}
/**
* Constructor
* @param Isotropic measurement noise
@ -197,8 +200,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,
@ -275,6 +278,43 @@ 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 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;
}
/**
* 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
@ -404,43 +444,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)
@ -457,8 +460,8 @@ class SmartProjectionPoseFactorRollingShutter
return this->createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: unknown linearization "
"mode");
"SmartProjectionPoseFactorRollingShutter: "
"unknown linearization mode");
}
}