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

Saved a transpose

release/4.3a0
dellaert 2015-02-21 11:00:13 +01:00
parent c7a41d30cc
commit 2ee090ece5
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/CalibratedCamera.cpp
View File

@ -34,13 +34,13 @@ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point; Matrix23 Dpn_point;
Dpn_point << // Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
Dpn_point *= d; Dpn_point *= d;
return Dpn_point; return Dpn_point;
} }
@ -118,7 +118,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
if (Dpose) if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d); *Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint) if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose *Dpoint = PinholeBase::Dpoint(pn, d, Rt);
} }
return pn; return pn;
} }

4
gtsam/geometry/CalibratedCamera.h
View File

@ -59,9 +59,9 @@ protected:
* Calculate Jacobian with respect to point * Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates * @param pn projection in normalized coordinates
* @param d disparity (inverse depth) * @param d disparity (inverse depth)
* @param R rotation matrix * @param Rt transposed rotation matrix
*/ */
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
/// @} /// @}