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

The copy/paste from CalibratedCamera (can we not avoid this?) had the side effect of not benefiting from the collapsing derivatives there. I added this in Pinhole now, which should shave off quite a bit of time in the derivative calculation.

release/4.3a0
Frank Dellaert 2013-10-12 06:31:28 +00:00
parent 1a20272fa2
commit 31beb98a36
2 changed files with 49 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/geometry/PinholeCamera.h
View File

@ -313,35 +313,43 @@ public:
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.transform_to(pw);
if (pc.z() <= 0)
throw CheiralityException();
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
// world to camera coordinate
Matrix Dpc_pose /* 3*6 */, Dpc_point /* 3*3 */;
const Point3 pc = pose_.transform_to(pw, Dpc_pose, Dpc_point);
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
if (pc.z() <= 0)
throw CheiralityException();
// camera to normalized image coordinate
Matrix Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
// uncalibration
Matrix Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dpose || Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb
const double z = pc.z(), d = 1.0 / z;
const double u = pn.x(), v = pn.y();
Matrix Dpn_pose(2, 6), Dpn_point(2, 3);
if (Dpose) {
double uv = u * v, uu = u * u, vv = v * v;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
}
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
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);
Dpn_point *= d;
}
// chain the Jacobian matrices
const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = Dpi_pc * Dpc_point;
return pi;
// uncalibration
Matrix Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
if (Dpose)
*Dpose = Dpi_pn * Dpn_pose;
if (Dpoint)
*Dpoint = Dpi_pn * Dpn_point;
return pi;
} else
return K_.uncalibrate(pn, Dcal);
}
/** project a point at infinity from world coordinate to the image
@ -531,4 +539,5 @@ private:
ar & BOOST_SERIALIZATION_NVP(K_);
}
/// @}
};}
}
;}

21
gtsam/geometry/tests/timePinholeCamera.cpp
View File

@ -45,8 +45,11 @@ int main()
*/
// Oct 12 2013, iMac 3.06GHz Core i3
// 447577 calls/second
// 2.23425 musecs/call
// 6.78564e+06 calls/second
// 0.14737 musecs/call
// And after collapse:
// 8.71916e+06 calls/second
// 0.11469 musecs/call
{
long timeLog = clock();
for(int i = 0; i < n; i++)
@ -58,8 +61,11 @@ int main()
}
// Oct 12 2013, iMac 3.06GHz Core i3
// 35367.5 calls/second
// 28.2746 musecs/call
// 258265 calls/second
// 3.87199 musecs/call
// And after collapse:
// 380686 calls/second
// 2.62684 musecs/call
{
Matrix Dpose, Dpoint;
long timeLog = clock();
@ -72,8 +78,11 @@ int main()
}
// Oct 12 2013, iMac 3.06GHz Core i3
// 34325.7 calls/second
// 29.1327 musecs/call
// 249258 calls/second
// 4.0119 musecs/call
// And after collapse:
// 389135 calls/second
// 2.5698 musecs/call
{
Matrix Dpose, Dpoint, Dcal;
long timeLog = clock();