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

cleanup in geometry

release/4.3a0
Chris Beall 2010-10-21 20:16:26 +00:00
parent cfc6387537
commit 8e40b28426
2 changed files with 24 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
geometry/StereoCamera.cpp
View File

@ -20,10 +20,11 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace gtsam{ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline) : StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K,
double baseline) :
leftCamPose_(leftCamPose), K_(K), baseline_(baseline) { leftCamPose_(leftCamPose), K_(K), baseline_(baseline) {
Vector calibration = K_.vector(); Vector calibration = K_.vector();
fx_ = calibration(0); fx_ = calibration(0);
@ -40,72 +41,52 @@ StereoPoint2 StereoCamera::project(const Point3& point,
Point3 cameraPoint = leftCamPose_.transform_to(point); Point3 cameraPoint = leftCamPose_.transform_to(point);
if (Dproject_stereo_pose) { if (Dproject_stereo_pose) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose,
boost::none);
//Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point); Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
//**** above function call inlined Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian *Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
} }
if (Dproject_stereo_point) { if (Dproject_stereo_point) {
//Point2 intrinsic = project(camera.calibrated_, point); // unused Matrix D_cameraPoint_point;
Point3 cameraPoint = pose().transform_to(point, boost::none,
D_cameraPoint_point);
//Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point); Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
//**** above function call inlined Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
Matrix D_cameraPoint_point;
Point3 cameraPoint = pose().transform_to(point, boost::none, D_cameraPoint_point);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
//****
//Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point; *Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
} }
double d = 1.0 / cameraPoint.z(); double d = 1.0 / cameraPoint.z();
double uL = cx_ + d * fx_ * cameraPoint.x(); double uL = cx_ + d * fx_ * cameraPoint.x();
double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_); double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_);
double v = cy_ + d * fy_ * cameraPoint.y(); double v = cy_ + d * fy_ * cameraPoint.y();
return StereoPoint2(uL,uR,v); return StereoPoint2(uL, uR, v);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d * d; double d = 1.0 / P.z(), d2 = d * d;
//return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - baseline()) * d2,
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x()-baseline()) * d2, 0.0, d, -P.y() * d2); 0.0, d, -P.y() * d2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) { Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
Vector calibration = K.vector(); // I want fx, fx, fy Vector calibration = K.vector();
Vector calibration2(3); Vector calibration2(3);
calibration2(0) = calibration(0); calibration2(0) = calibration(0);
calibration2(1) = calibration(0); calibration2(1) = calibration(0);
calibration2(2) = calibration(1); calibration2(2) = calibration(1);
return diag(calibration2); return diag(calibration2);
} }
} }

3
geometry/tests/testStereoCamera.cpp
View File

@ -34,7 +34,6 @@ TEST( StereoCamera, operators)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( StereoCamera, project) TEST( StereoCamera, project)
{ {
double uL, uR, v;
// create a Stereo camera at the origin with focal length 1500, baseline 0.5m // create a Stereo camera at the origin with focal length 1500, baseline 0.5m
// and principal point 320, 240 (for a hypothetical 640x480 sensor) // and principal point 320, 240 (for a hypothetical 640x480 sensor)
Cal3_S2 K(1500, 1500, 0, 320, 240); Cal3_S2 K(1500, 1500, 0, 320, 240);
@ -67,8 +66,6 @@ TEST( StereoCamera, project)
StereoCamera stereoCam4(camPose4, K, 0.5); StereoCamera stereoCam4(camPose4, K, 0.5);
StereoPoint2 result4 = stereoCam4.project(p4); StereoPoint2 result4 = stereoCam4.project(p4);
CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result4)); CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result4));
// cout << "(uL,uR,v): ("<<result4(0)<<","<<result4(1)<<","<<result4(2)<<")" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */