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

Speeding up localCoordinates

release/4.3a0
dellaert 2014-10-22 13:46:05 +02:00
parent 1061a66fc1
commit e46be60215
3 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3Bundler.cpp
View File

@ -46,7 +46,7 @@ Vector Cal3Bundler::k() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Bundler::vector() const { Vector3 Cal3Bundler::vector() const {
return (Vector(3) << f_, k1_, k2_); return (Vector(3) << f_, k1_, k2_);
} }
@ -189,7 +189,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
return T2.vector() - vector(); return T2.vector() - vector();
} }

4
gtsam/geometry/Cal3Bundler.h
View File

@ -70,7 +70,7 @@ public:
Matrix K() const; ///< Standard 3*3 calibration matrix Matrix K() const; ///< Standard 3*3 calibration matrix
Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector vector() const; Vector3 vector() const;
/// focal length x /// focal length x
inline double fx() const { inline double fx() const {
@ -150,7 +150,7 @@ public:
Cal3Bundler retract(const Vector& d) const; Cal3Bundler retract(const Vector& d) const;
/// Calculate local coordinates to another calibration /// Calculate local coordinates to another calibration
Vector localCoordinates(const Cal3Bundler& T2) const; Vector3 localCoordinates(const Cal3Bundler& T2) const;
/// dimensionality /// dimensionality
virtual size_t dim() const { virtual size_t dim() const {

11
gtsam/geometry/PinholeCamera.h
View File

@ -242,12 +242,13 @@ public:
calibration().retract(d.tail(calibration().dim()))); calibration().retract(d.tail(calibration().dim())));
} }
typedef Eigen::Matrix<double,6+DimK,1> VectorK6;
/// return canonical coordinate /// return canonical coordinate
Vector localCoordinates(const PinholeCamera& T2) const { VectorK6 localCoordinates(const PinholeCamera& T2) const {
Vector d(dim()); VectorK6 d; // TODO: why does d.head<6>() not compile??
d.head(pose().dim()) = pose().localCoordinates(T2.pose()); d.head(6) = pose().localCoordinates(T2.pose());
d.tail(calibration().dim()) = calibration().localCoordinates( d.tail(DimK) = calibration().localCoordinates(T2.calibration());
T2.calibration());
return d; return d;
} }