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

collapsed derivatives for threefold speedup

release/4.3a0
Frank Dellaert 2010-08-08 19:50:41 +00:00
parent 4b6eb67340
commit 62c63f9452
2 changed files with 73 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
geometry/CalibratedCamera.cpp
View File

@ -64,43 +64,49 @@ namespace gtsam {
/* ************************************************************************* */
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point);
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
return D_intrinsic_pose;
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -d*r1.x()+r3.x()*Xd2, -d*r1.y()+r3.y()*Xd2, -d*r1.z()+r3.z()*Xd2,
d*Z+Y*Yd2, -X*Yd2, -d*X, -d*r2.x()+r3.x()*Yd2, -d*r2.y()+r3.y()*Yd2, -d*r2.z()+r3.z()*Yd2);
}
/* ************************************************************************* */
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
Point3 cameraPoint = transform_to(camera.pose(),point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point);
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
return D_intrinsic_point;
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
return Matrix_(2,3,
d*r1.x()-r3.x()*Xd2, d*r1.y()-r3.y()*Xd2, d*r1.z()-r3.z()*Xd2,
d*r2.x()-r3.x()*Yd2, d*r2.y()-r3.y()*Yd2, d*r2.z()-r3.z()*Yd2);
}
/* ************************************************************************* */
Point2 Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(),point); // 3*6
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point); // 3*3
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; // 2*6
D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; // 2*3
return intrinsic;
const Pose3& pose = camera.pose();
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = transform_to(pose, point);
double X = q.x(), Y = q.y(), Z = q.z();
double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2;
double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2;
double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2;
D_intrinsic_pose = Matrix_(2,6,
X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13,
d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23);
D_intrinsic_point = Matrix_(2,3,
dp11, dp12, dp13,
dp21, dp22, dp23);
return project_to_camera(q);
}
/* ************************************************************************* */

45
geometry/timeCalibratedCamera.cpp
View File

@ -15,7 +15,6 @@ using namespace gtsam;
int main()
{
int n = 100000;
Matrix computed;
const Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
@ -28,10 +27,13 @@ int main()
const Point3 point1(-0.08,-0.08, 0.0);
// Aug 8, iMac 3.06GHz Core i3
// 0.263943 seconds
// 378870 calls/second
// 2.63943 musecs/call
// 378870 calls/second
// 2.63943 musecs/call
// AFTER collapse:
// 1.57399e+06 calls/second
// 0.63533 musecs/call
{
Matrix computed;
long timeLog = clock();
for(int i = 0; i < n; i++)
computed = Dproject_pose(camera, point1);
@ -39,6 +41,39 @@ int main()
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
}
// Aug 8, iMac 3.06GHz Core i3
// AFTER collapse:
// 1.55383e+06 calls/second
// 0.64357 musecs/call
{
Matrix computed;
long timeLog = clock();
for(int i = 0; i < n; i++)
computed = Dproject_point(camera, point1);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
}
// Aug 8, iMac 3.06GHz Core i3
// 371153 calls/second
// 2.69431 musecs/call
// AFTER collapse:
// 1.10733e+06 calls/second
// 0.90307 musecs/call
{
Matrix computed1, computed2;
long timeLog = clock();
for(int i = 0; i < n; i++)
Dproject_pose_point(camera, point1, computed1, computed2);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
}
return 0;
}