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

Used more fixed-size math

release/4.3a0
dellaert 2015-03-10 18:57:56 -07:00
parent fcc2470e28
commit 28bb4f9673
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/PinholeCamera.h
View File

@ -145,7 +145,7 @@ public:
const Pose3& getPose(OptionalJacobian<6, dimension> H) const { const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) { if (H) {
H->setZero(); H->setZero();
H->block(0, 0, 6, 6) = I_6x6; H->template block<6,6>(0, 0) = I_6x6;
} }
return Base::pose(); return Base::pose();
} }
@ -176,16 +176,15 @@ public:
if ((size_t) d.size() == 6) if ((size_t) d.size() == 6)
return PinholeCamera(this->pose().retract(d), calibration()); return PinholeCamera(this->pose().retract(d), calibration());
else else
return PinholeCamera(this->pose().retract(d.head(6)), return PinholeCamera(this->pose().retract(d.head<6>()),
calibration().retract(d.tail(calibration().dim()))); calibration().retract(d.tail(calibration().dim())));
} }
/// return canonical coordinate /// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d; VectorK6 d;
// TODO: why does d.head<6>() not compile?? d.template head<6>() = this->pose().localCoordinates(T2.pose());
d.head(6) = this->pose().localCoordinates(T2.pose()); d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
return d; return d;
} }
@ -265,7 +264,7 @@ public:
if (Dother) { if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension); Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero(); Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_; Dother->block<1,6>(0, 0) = Dother_;
} }
return result; return result;
} }