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

Clear and functional

release/4.3a0
Frank Dellaert 2019-01-04 15:10:47 -05:00
parent cb3e688fa5
commit 6c59273a18
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -123,18 +123,16 @@ public:
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
for(const Key& k: this->keys_) {
Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
Camera camera(pose, K_);
cameras.push_back(camera);
for (const Key& k : this->keys_) {
const Pose3 world_P_sensor_k =
Base::body_P_sensor_ ? values.at<Pose3>(k) * *Base::body_P_sensor_
: values.at<Pose3>(k);
cameras.emplace_back(world_P_sensor_k, K_);
}
return cameras;
}
private:
private:
/// Serialization function
friend class boost::serialization::access;