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

Optimized StereoCamera.project derivative

release/4.3a0
Frank Dellaert 2012-01-08 05:09:11 +00:00
parent a032a93cf7
commit 9b5ac235e2
5 changed files with 138 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.cproject
View File

@ -1507,10 +1507,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/timeCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeCalibratedCamera.run</buildTarget>
<buildTarget>tests/timeStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

2
gtsam/geometry/Makefile.am
View File

@ -33,7 +33,7 @@ sources += projectiveGeometry.cpp tensorInterface.cpp
check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
# Timing tests
noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera
noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
# Rot3M and Rot3Q both use Rot3.h, they do not have individual header files
allsources = $(sources)

12
gtsam/geometry/SimpleCamera.cpp
View File

@ -49,18 +49,14 @@ namespace gtsam {
Point2 intrinsic = calibrated_.project(point, H1, H2);
if (!H1 && !H2)
return K_.uncalibrate(intrinsic);
else {
Matrix D_projection_intrinsic;
Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
if (H1) {
*H1 = D_projection_intrinsic * (*H1);
}
if (H2) {
*H2 = D_projection_intrinsic * (*H2);
}
if (H1) *H1 = D_projection_intrinsic * (*H1);
if (H2) *H2 = D_projection_intrinsic * (*H2);
return projection;
}
}
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);

72
gtsam/geometry/StereoCamera.cpp
View File

@ -23,33 +23,64 @@ using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) :
StereoCamera::StereoCamera(const Pose3& leftCamPose,
const Cal3_S2Stereo::shared_ptr K) :
leftCamPose_(leftCamPose), K_(K) {
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2);
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
#else
// omit derivatives
const Point3 q = leftCamPose_.transform_to(point);
#endif
// get calibration
const Cal3_S2Stereo& K = *K_;
const double fx = K.fx(), fy = K.fy(), b = K.baseline();
// calculate scaled but not translated image coordinates
const double d = 1.0 / q.z();
const double x = q.x(), y = q.y();
const double uL = d*fx*x;
const double uR = d*fx*(x - b);
const double v = d*fy*y;
// check if derivatives need to be computed
if (H1 || H2) {
Matrix D_project_point = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
if (H1)
*H1 = D_project_point * *H1;
if (H2)
*H2 = D_project_point * *H2;
#ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule
Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2);
#else
// optimized version, see StereoCamera.nb
if (H1) {
const double z = q.z(), fxz = fx*z, v1 = v/fy, v2 = fx*v1;
const double dfx = d*fx, dx = d*x;
*H1 = Matrix_(3, 6,
uL*v1, -d*(uL*x + fxz), v2, -dfx, 0.0, d*uL,
uR*v1, -d*(uR*x + fxz), v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v, -dx*fy, 0.0, -d*fy, d*v
);
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
*H2 = d * Matrix_(3, 3,
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
);
}
#endif
}
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b=K.baseline();
double d = 1.0 / cameraPoint.z();
double uL = K.px() + d * f_x * cameraPoint.x();
double uR = K.px() + d * f_x * (cameraPoint.x() - b);
double v = K.py() + d * f_y * cameraPoint.y();
return StereoPoint2(uL, uR, v);
// finally translate
return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v);
}
/* ************************************************************************* */
@ -58,9 +89,10 @@ Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
return Matrix_(3, 3,
f_x*d, 0.0, -f_x *P.x() * d2,
f_x*d, 0.0, -f_x *(P.x() - b) * d2,
0.0, f_y*d, -f_y*P.y() * d2);
f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y()
);
}
}

51
gtsam/geometry/tests/timeStereoCamera.cpp Normal file
View File

@ -0,0 +1,51 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file timeStereoCamera.cpp
* @brief time StereoCamera derivatives
* @author Frank Dellaert
*/
#include <time.h>
#include <iostream>
#include <gtsam/geometry/StereoCamera.h>
using namespace std;
using namespace gtsam;
int main()
{
int n = 100000;
const Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,0.5));
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
const StereoCamera camera(pose1, K);
const Point3 point1(-0.08,-0.08, 0.0);
Matrix computed1, computed2;
long timeLog = clock();
for(int i = 0; i < n; i++)
camera.project(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;
}