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

StereoCamera cleanup and improvements

release/4.3a0
Chris Beall 2011-09-12 12:37:10 +00:00
parent b456733cd0
commit 6da5127981
6 changed files with 39 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -30,6 +30,9 @@ namespace gtsam {
double b_;
public:
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
/**
* default calibration leaves coordinates unchanged
*/

45
gtsam/geometry/StereoCamera.cpp
View File

@ -23,7 +23,7 @@ using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) :
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) :
leftCamPose_(leftCamPose), K_(K) {
}
@ -32,49 +32,40 @@ StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point3 cameraPoint = leftCamPose_.transform_to(point);
if (H1) {
Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose,
boost::none);
Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2);
if(H1 || H2) {
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(K_); // 3x3
Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3
*H1 = D_projection_intrinsic * D_intrinsic_pose;
}
if (H1) {
//Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * *H1;
*H1 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H1;
}
if (H2) {
Matrix D_cameraPoint_point;
Point3 cameraPoint = pose().transform_to(point, boost::none,
D_cameraPoint_point);
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(calibration()); // 3x3
*H2 = D_projection_intrinsic * D_intrinsic_point;
if (H2) {
//Matrix D_intrinsic_point = D_intrinsic_cameraPoint * *H2;
*H2 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H2;
}
}
double d = 1.0 / cameraPoint.z();
double uL = K_.px() + d * K_.fx() * cameraPoint.x();
double uR = K_.px() + d * K_.fx() * (cameraPoint.x() - K_.baseline());
double v = K_.py() + d * K_.fy() * cameraPoint.y();
double uL = K_->px() + d * K_->fx() * cameraPoint.x();
double uR = K_->px() + d * K_->fx() * (cameraPoint.x() - K_->baseline());
double v = K_->py() + d * K_->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_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_.baseline()) * d2,
return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_->baseline()) * d2,
0.0, d, -P.y() * d2);
}
/* ************************************************************************* */
Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
Vector calibration = K.vector();
Matrix StereoCamera::Duncalibrate2(const Cal3_S2Stereo::shared_ptr K) {
Vector calibration = K->vector();
Vector calibration2(3);
calibration2(0) = calibration(0);
calibration2(1) = calibration(0);

26
gtsam/geometry/StereoCamera.h
View File

@ -32,15 +32,15 @@ namespace gtsam {
private:
Pose3 leftCamPose_;
Cal3_S2Stereo K_;
Cal3_S2Stereo::shared_ptr K_;
public:
StereoCamera() {
}
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K);
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
const Cal3_S2Stereo& calibration() const {
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
}
@ -49,7 +49,7 @@ namespace gtsam {
}
const double baseline() const {
return K_.baseline();
return K_->baseline();
}
@ -75,16 +75,16 @@ namespace gtsam {
* i.e. does not rely on baseline
*/
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
Point2 intrinsic = K_->calibrate(projection);
Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
return pose().transform_from(cameraPoint);
}
Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
double X = Z *(measured[0]- K_.px()) / K_.fx();
double Y = Z *(measured[2]- K_.py()) / K_.fy();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
double X = Z *(measured[0]- K_->px()) / K_->fx();
double Y = Z *(measured[2]- K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
@ -101,7 +101,7 @@ namespace gtsam {
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d), calibration());
return StereoCamera(pose().expmap(d), K_);
}
Vector logmap(const StereoCamera &camera) const {
@ -110,8 +110,8 @@ namespace gtsam {
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals(
camera.K_, tol);
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
}
Pose3 between(const StereoCamera &camera,
@ -122,13 +122,13 @@ namespace gtsam {
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.");
K_.print(s + ".calibration.");
K_->print(s + ".calibration.");
}
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
static Matrix Duncalibrate2(const Cal3_S2& K);
static Matrix Duncalibrate2(const Cal3_S2Stereo::shared_ptr K);
friend class boost::serialization::access;
template<class Archive>

6
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -36,7 +36,7 @@ TEST( StereoCamera, project)
{
// 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_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
@ -77,7 +77,7 @@ Pose3 camera1(Matrix_(3,3,
),
Point3(0,0,6.25));
Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
@ -115,7 +115,7 @@ TEST( StereoCamera, backproject2)
-0.804435942, -0.592650676, -0.0405925523,
0.0732045588, -0.0310882277, -0.996832359);
Point3 t(53.5239823, 23.7866016, -4.42379876);
Cal3_S2Stereo K(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
StereoCamera camera(Pose3(R,t), K);
StereoPoint2 z(184.812, 129.068, 714.768);

2
gtsam/slam/StereoFactor.h
View File

@ -86,7 +86,7 @@ public:
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, *K_);
StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - z_).vector();
}

2
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -40,7 +40,7 @@ Pose3 camera1(Matrix_(3,3,
),
Point3(0,0,6.25));
Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters