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
const double u = pn.x(), v = pn.y();
Matrix23 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), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(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;
return Dpn_point;
}
@ -118,7 +118,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
if (Dpose)
*Dpose = PinholeBase::Dpose(pn, d);
if (Dpoint)
*Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
*Dpoint = PinholeBase::Dpoint(pn, d, Rt);
}
return pn;
}

4
gtsam/geometry/CalibratedCamera.h
View File

@ -59,9 +59,9 @@ protected:
* Calculate Jacobian with respect to point
* @param pn projection in normalized coordinates
* @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);
/// @}