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

Some refactoring in Cal3Bundler

release/4.3a0
Frank Dellaert 2013-10-12 15:04:26 +00:00
parent a423849afe
commit 1f293294fd
4 changed files with 48 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -1653,6 +1653,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3Bundler.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

28
gtsam/geometry/Cal3Bundler.cpp
View File

@ -74,9 +74,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double r2 = r * r;
const double g = 1 + (k1_ + k2_ * r) * r;
const double fg = f_ * g;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
const double f = f_;
// semantic meaningful version
//if (H1) *H1 = D2d_calibration(p);
@ -84,26 +84,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
// unrolled version, much faster
if (Dcal || Dp) {
double rx = r * x, ry = r * y;
const double fx = f_ * x, fy = f_ * y;
if (Dcal) {
*Dcal = Matrix_(2, 3, //
g * x, fx * r, fx * r2, //
g * y, fy * r, fy * r2);
Eigen::Matrix<double, 2, 3> D;
D << u, f * rx, f * r * rx, v, f * ry, f * r * ry;
*Dcal = D;
}
if (Dp) {
const double dr_dx = 2 * x;
const double dr_dy = 2 * y;
const double dg_dx = k1_ * dr_dx + k2_ * 2 * r * dr_dx;
const double dg_dy = k1_ * dr_dy + k2_ * 2 * r * dr_dy;
*Dp = Matrix_(2, 2, //
fg + fx * dg_dx, fx * dg_dy, //
fy * dg_dx, fg + fy * dg_dy);
const double dg_dx = 2. * k1_ * x + 4. * k2_ * rx;
const double dg_dy = 2. * k1_ * y + 4. * k2_ * ry;
Eigen::Matrix<double, 2, 2> D;
D << g + x * dg_dx, x * dg_dy, y * dg_dx, g + y * dg_dy;
*Dp = f * D;
}
}
return Point2(fg * x, fg * y);
return Point2(f * u, f * v);
}
/* ************************************************************************* */

4
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -45,7 +45,9 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
TEST( Cal3Bundler, Duncalibrate)
{
Matrix Dcal, Dp;
K.uncalibrate(p, Dcal, Dp);
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(1182, 1773);
CHECK(assert_equal(expected,actual,1e-7));
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical1,Dcal,1e-7));

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

@ -19,14 +19,14 @@
#include <iostream>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
using namespace std;
using namespace gtsam;
int main()
{
int n = 100000;
int n = 1000000;
const Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
@ -35,8 +35,10 @@ int main()
),
Point3(0,0,0.5));
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
const PinholeCamera<Cal3DS2> camera(pose1,K);
// static Cal3_S2 K(500, 100, 0.1, 320, 240);
// static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
const PinholeCamera<Cal3Bundler> camera(pose1,K);
const Point3 point1(-0.08,-0.08, 0.0);
/**
@ -45,17 +47,12 @@ int main()
*/
// Oct 12 2013, iMac 3.06GHz Core i3
// 6.78564e+06 calls/second
// 0.14737 musecs/call
// And after collapse:
// 8.71916e+06 calls/second
// 0.11469 musecs/call
// Cal3DS2:
// 7.04176e+06 calls/second
// 0.14201 musecs/call
// After Cal3DS2 fix:
// 8.17595e+06 calls/second
// 0.12231 musecs/call
// Original: 0.14737 musecs/call
// After collapse: 0.11469 musecs/call
// Cal3DS2: 0.14201 musecs/call
// After Cal3DS2 fix: 0.12231 musecs/call
// Cal3Bundler: 0.12000 musecs/call
// Cal3Bundler fix: 0.13864 musecs/call
{
long timeLog = clock();
for(int i = 0; i < n; i++)
@ -67,17 +64,12 @@ int main()
}
// Oct 12 2013, iMac 3.06GHz Core i3
// 258265 calls/second
// 3.87199 musecs/call
// And after collapse:
// 380686 calls/second
// 2.62684 musecs/call
// Cal3DS2:
// 230789 calls/second
// 4.33297 musecs/call
// After Cal3DS2 fix:
// 304354 calls/second
// 3.28565 musecs/call
// Original: 3.87199 musecs/call
// After collapse: 2.62684 musecs/call
// Cal3DS2: 4.33297 musecs/call
// After Cal3DS2 fix: 3.28565 musecs/call
// Cal3Bundler: 2.65559 musecs/call
// Cal3Bundler fix: 2.18481 musecs/call
{
Matrix Dpose, Dpoint;
long timeLog = clock();
@ -90,17 +82,12 @@ int main()
}
// Oct 12 2013, iMac 3.06GHz Core i3
// 249258 calls/second
// 4.0119 musecs/call
// And after collapse:
// 389135 calls/second
// 2.5698 musecs/call
// Cal3DS2:
// 206933 calls/second
// 4.83248 musecs/call
// After Cal3DS2 fix:
// 289996 calls/second
// 3.44832 musecs/call
// Original: 4.0119 musecs/call
// After collapse: 2.5698 musecs/call
// Cal3DS2: 4.83248 musecs/call
// After Cal3DS2 fix: 3.44832 musecs/call
// Cal3Bundler: 2.51124 musecs/call
// Cal3Bundler fix: 2.17292 musecs/call
{
Matrix Dpose, Dpoint, Dcal;
long timeLog = clock();