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 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) {
Vector calibration = K_.vector();
fx_ = calibration(0);
@ -40,72 +41,52 @@ StereoPoint2 StereoCamera::project(const Point3& point,
Point3 cameraPoint = leftCamPose_.transform_to(point);
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);
//**** above function call inlined
Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none);
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
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;
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
}
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);
//**** above function call inlined
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_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
Matrix D_projection_intrinsic = Duncalibrate2(K()); // 3x3
*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
}
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 v = cy_ + d * fy_ * cameraPoint.y();
return StereoPoint2(uL,uR,v);
double v = cy_ + d * fy_ * cameraPoint.y();
return StereoPoint2(uL, uR, v);
}
/* ************************************************************************* */
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
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, 0.0, d, -P.y() * d2);
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - baseline()) * d2,
0.0, d, -P.y() * d2);
}
/* ************************************************************************* */
Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
Vector calibration = K.vector(); // I want fx, fx, fy
Vector calibration = K.vector();
Vector calibration2(3);
calibration2(0) = calibration(0);
calibration2(1) = calibration(0);
calibration2(2) = calibration(1);
return diag(calibration2);
}
}
}

3
geometry/tests/testStereoCamera.cpp
View File

@ -34,7 +34,6 @@ TEST( StereoCamera, operators)
/* ************************************************************************* */
TEST( StereoCamera, project)
{
double uL, uR, v;
// create a Stereo camera at the origin with focal length 1500, baseline 0.5m
// and principal point 320, 240 (for a hypothetical 640x480 sensor)
Cal3_S2 K(1500, 1500, 0, 320, 240);
@ -67,8 +66,6 @@ TEST( StereoCamera, project)
StereoCamera stereoCam4(camPose4, K, 0.5);
StereoPoint2 result4 = stereoCam4.project(p4);
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;
}
/* ************************************************************************* */