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

Made code a bit more efficient in case of offset

release/4.3a0
Frank Dellaert 2019-08-08 11:52:33 -04:00
parent 214dca3aa5
commit 0eef77ff36
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartFactorBase.h
View File

@ -75,15 +75,12 @@ protected:
*/ */
ZVector measured_; ZVector measured_;
/// @name Pose of the camera in the body frame
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
/// @}
// Cache for Fblocks, to avoid a malloc ever time we re-linearize // Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable FBlocks Fs; mutable FBlocks Fs;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
@ -200,17 +197,20 @@ public:
return e && Base::equals(p, tol) && areMeasurementsEqual; return e && Base::equals(p, tol) && areMeasurementsEqual;
} }
/// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
/// derivatives
template <class POINT> template <class POINT>
Vector unwhitenedError(const Cameras& cameras, const POINT& point, Vector unwhitenedError(
const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, // boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E); Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if (body_P_sensor_ && Fs) { if (body_P_sensor_ && Fs) {
const Pose3 sensor_P_body = body_P_sensor_->inverse();
for (size_t i = 0; i < Fs->size(); i++) { for (size_t i = 0; i < Fs->size(); i++) {
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
Matrix J(6, 6); Matrix J(6, 6);
Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
Fs->at(i) = Fs->at(i) * J; Fs->at(i) = Fs->at(i) * J;
} }
} }