Optimized StereoCamera.project derivative
Frank Dellaert
parent
a032a93cf7
commit
9b5ac235e2
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -49,17 +49,13 @@ namespace gtsam {
|
|||
Point2 intrinsic = calibrated_.project(point, H1, H2);
|
||||
if (!H1 && !H2)
|
||||
return K_.uncalibrate(intrinsic);
|
||||
|
||||
Matrix D_projection_intrinsic;
|
||||
Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
|
||||
|
||||
if (H1) {
|
||||
*H1 = D_projection_intrinsic * (*H1);
|
||||
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);
|
||||
return projection;
|
||||
}
|
||||
if (H2) {
|
||||
*H2 = D_projection_intrinsic * (*H2);
|
||||
}
|
||||
return projection;
|
||||
}
|
||||
|
||||
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
|
||||
|
|
|
|||
|
|
@ -1,18 +1,18 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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)
|
||||
*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
|
||||
*See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
*-------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file StereoCamera.h
|
||||
* @brief A Stereo Camera based on two Simple Cameras
|
||||
* @author Chris Beall
|
||||
*@file StereoCamera.h
|
||||
*@brief A Stereo Camera based on two Simple Cameras
|
||||
*@author Chris Beall
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
|
|
@ -22,45 +22,77 @@ using namespace gtsam;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
|
||||
Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2);
|
||||
|
||||
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;
|
||||
/* ************************************************************************* */
|
||||
StereoCamera::StereoCamera(const Pose3& leftCamPose,
|
||||
const Cal3_S2Stereo::shared_ptr K) :
|
||||
leftCamPose_(leftCamPose), K_(K) {
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
StereoPoint2 StereoCamera::project(const Point3& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
|
||||
double d = 1.0 / P.z(), d2 = d * d;
|
||||
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);
|
||||
}
|
||||
#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) {
|
||||
#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
|
||||
}
|
||||
|
||||
// finally translate
|
||||
return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
|
||||
double d = 1.0 / P.z(), d2 = d*d;
|
||||
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, -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()
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue