diff --git a/geometry/CalibratedCamera.cpp b/geometry/CalibratedCamera.cpp index 0bef6cdde2..e979896938 100644 --- a/geometry/CalibratedCamera.cpp +++ b/geometry/CalibratedCamera.cpp @@ -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); } /* ************************************************************************* */ diff --git a/geometry/timeCalibratedCamera.cpp b/geometry/timeCalibratedCamera.cpp index 96b9f862fd..34251e0b93 100644 --- a/geometry/timeCalibratedCamera.cpp +++ b/geometry/timeCalibratedCamera.cpp @@ -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; }