From 6694b395c59c8316515747fcaf8cf2a6bf2aa481 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Aug 2010 20:23:38 +0000 Subject: [PATCH] moved Stereo camera and StereoPoint to gtsam --- geometry/Makefile.am | 4 + geometry/StereoCamera.cpp | 128 ++++++++++++++++++++++++++++ geometry/StereoCamera.h | 71 +++++++++++++++ geometry/StereoPoint2.cpp | 9 ++ geometry/StereoPoint2.h | 101 ++++++++++++++++++++++ geometry/tests/testStereoCamera.cpp | 96 +++++++++++++++++++++ 6 files changed, 409 insertions(+) create mode 100644 geometry/StereoCamera.cpp create mode 100644 geometry/StereoCamera.h create mode 100644 geometry/StereoPoint2.cpp create mode 100644 geometry/StereoPoint2.h create mode 100644 geometry/tests/testStereoCamera.cpp diff --git a/geometry/Makefile.am b/geometry/Makefile.am index e7ac5db7f..01547fda9 100644 --- a/geometry/Makefile.am +++ b/geometry/Makefile.am @@ -14,6 +14,10 @@ check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoin sources += Cal3_S2.cpp CalibratedCamera.cpp SimpleCamera.cpp check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera +# Stereo +sources += StereoPoint2.cpp StereoCamera.cpp +check_PROGRAMS += tests/testStereoCamera + # Tensors headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h diff --git a/geometry/StereoCamera.cpp b/geometry/StereoCamera.cpp new file mode 100644 index 000000000..98999f2fb --- /dev/null +++ b/geometry/StereoCamera.cpp @@ -0,0 +1,128 @@ +/** + * @file StereoCamera.h + * @brief A Stereo Camera based on two Simple Cameras + * @author Chris Beall + */ + +#include "StereoCamera.h" + +using namespace std; +using namespace gtsam; + +namespace gtsam{ + +/* ************************************************************************* */ +StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline) : + leftCamPose_(leftCamPose), K_(K), baseline_(baseline) { + Vector calibration = K_.vector(); + fx_ = calibration(0); + fy_ = calibration(1); + cx_ = calibration(3); + cy_ = calibration(4); +} + +/* ************************************************************************* */ +StereoPoint2 StereoCamera::project(const Point3& point) const { + + Point3 cameraPoint = transform_to(leftCamPose_, point); + + double d = 1.0 / cameraPoint.z(); + 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); +} + +/* ************************************************************************* */ +Matrix Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P) { + 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()-camera.baseline()) * d2, 0.0, d, -P.y() * d2); +} + +/* ************************************************************************* */ +Matrix Duncalibrate2(const Cal3_S2& K) { + Vector calibration = K.vector(); // I want fx, fx, fy + Vector calibration2(3); + calibration2(0) = calibration(0); + calibration2(1) = calibration(0); + calibration2(2) = calibration(1); + return diag(calibration2); + //return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_); +} + +/* ************************************************************************* */ +Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) { + //Point2 intrinsic = project(camera.calibrated_, point); // unused + + //Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point); + //**** above function call inlined + Point3 cameraPoint = transform_to(camera.pose(), point); + Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3x6 + //cout << "D_cameraPoint_pose" << endl; + //print(D_cameraPoint_pose); + + //Point2 intrinsic = project_to_camera(cameraPoint); // unused + Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian + //cout << "myJacobian" << endl; + //print(D_intrinsic_cameraPoint); + + Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; + + //**** + //Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic); + Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3 + + Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose; + return D_projection_pose; +} + +/* ************************************************************************* */ +Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) { + //Point2 intrinsic = project(camera.calibrated_, point); // unused + + //Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point); + //**** above function call inlined + Point3 cameraPoint = transform_to(camera.pose(), point); + Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(), point); + + //Point2 intrinsic = project_to_camera(cameraPoint); // unused + Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, 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(camera.K()); // 3x3 + + Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point; + return D_projection_point; +} + + +// calibrated cameras +/* +Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) { + Point3 cameraPoint = transform_to(camera.pose(), point); + Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); + + Point2 intrinsic = project_to_camera(cameraPoint); + Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint); + + Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; + return D_intrinsic_pose; + } + +Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) { + Point3 cameraPoint = transform_to(camera.pose(), point); + Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); + + Point2 intrinsic = project_to_camera(cameraPoint); + Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint); + + Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; + return D_intrinsic_point; + } + */ + +} diff --git a/geometry/StereoCamera.h b/geometry/StereoCamera.h new file mode 100644 index 000000000..3dc69c212 --- /dev/null +++ b/geometry/StereoCamera.h @@ -0,0 +1,71 @@ +/** + * @file StereoCamera.h + * @brief A Stereo Camera based on two Simple Cameras + * @author Chris Beall + */ + +#pragma once + +#include "boost/tuple/tuple.hpp" +#include +#include +#include +#include "StereoPoint2.h" + +namespace gtsam { + + /** + * A stereo camera class + */ + class StereoCamera { + + private: + Pose3 leftCamPose_; + Cal3_S2 K_; + double baseline_; + double fx_, fy_; + double cx_, cy_; + public: + StereoCamera() { + } + + StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline); + + const Cal3_S2& K() const { + return K_; + } + + const Pose3& pose() const { + return leftCamPose_; + } + + const double baseline() const { + return baseline_; + } + + StereoPoint2 project(const Point3& point) const; + + }; + + /** Dimensionality of the tangent space */ + inline size_t dim(const StereoCamera& obj) { + return 6; + } + + /** Exponential map around p0 */ + template<> inline StereoCamera expmap(const StereoCamera& p0, const Vector& d) { + return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline()); + } + + inline StereoPoint2 project(const StereoCamera& camera, const Point3& point) { + return camera.project(point); + } + + Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point); + Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point); + + Matrix + Dproject_to_stereo_camera1(const StereoCamera& camera, const Point3& P); + Matrix Duncalibrate2(const Cal3_S2& K); + +} diff --git a/geometry/StereoPoint2.cpp b/geometry/StereoPoint2.cpp new file mode 100644 index 000000000..ee7141a5e --- /dev/null +++ b/geometry/StereoPoint2.cpp @@ -0,0 +1,9 @@ +/* + * StereoPoint2.cpp + * + * Created on: Jan 26, 2010 + * Author: dellaert + */ + +#include "StereoPoint2.h" + diff --git a/geometry/StereoPoint2.h b/geometry/StereoPoint2.h new file mode 100644 index 000000000..74e9c6e26 --- /dev/null +++ b/geometry/StereoPoint2.h @@ -0,0 +1,101 @@ +/* + * StereoPoint2.h + * + * Created on: Jan 26, 2010 + * Author: dellaert + */ + +#ifndef STEREOPOINT2_H_ +#define STEREOPOINT2_H_ + +#include +#include +#include +#include +#include + +namespace gtsam { + + /** + * A 2D stereo point, v will be same for rectified images + */ + class StereoPoint2: Testable, Lie { + private: + double uL_, uR_, v_; + + public: + + /** default constructor */ + StereoPoint2() : + uL_(0), uR_(0), v_(0) { + } + + /** constructor */ + StereoPoint2(double uL, double uR, double v) : + uL_(uL), uR_(uR), v_(v) { + } + + /** print */ + void print(const std::string& s) const { + std::cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" + << std::endl; + } + + /** equals */ + bool equals(const StereoPoint2& q, double tol) const { + return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ + - q.v_) < tol); + } + + /** convert to vector */ + Vector vector() const { + return Vector_(3, uL_, uR_, v_); + } + + /** add two stereo points */ + StereoPoint2 operator+(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); + } + + /** subtract two stereo points */ + StereoPoint2 operator-(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); + } + + /* + * convenient function to get a Point2 from the left image + */ + inline Point2 point2(){ + return Point2(uL_, v_); + } + }; + + /** Dimensionality of the tangent space */ + inline size_t dim(const StereoPoint2& obj) { + return 3; + } + + /** Exponential map around identity - just create a Point2 from a vector */ + template<> inline StereoPoint2 expmap(const Vector& d) { + return StereoPoint2(d(0), d(1), d(2)); + } + + /** Log map around identity - just return the Point2 as a vector */ + inline Vector logmap(const StereoPoint2& p) { + return p.vector(); + } + + /** "Compose", just adds the coordinates of two points. */ + inline StereoPoint2 compose(const StereoPoint2& p1, const StereoPoint2& p0) { + return p0 + p1; + } + + /** inverse */ + inline StereoPoint2 inverse(const StereoPoint2& p) { + return StereoPoint2()-p; + } + + +} + +#endif /* STEREOPOINT2_H_ */ diff --git a/geometry/tests/testStereoCamera.cpp b/geometry/tests/testStereoCamera.cpp new file mode 100644 index 000000000..ae24c68c7 --- /dev/null +++ b/geometry/tests/testStereoCamera.cpp @@ -0,0 +1,96 @@ +/** + * @file testStereoCamera.cpp + * @brief Unit test for StereoCamera + * single camera + * @author Chris Beall + */ + +#include +#include +#include "StereoCamera.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( StereoCamera, operators) +{ + StereoPoint2 a(1, 2, 3), b(4, 5, 6), c(5, 7, 9), d(3, 3, 3); + CHECK(assert_equal(c,a+b)); + CHECK(assert_equal(d,b-a)); +} + +/* ************************************************************************* */ +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); + StereoCamera stereoCam(Pose3(), K, 0.5); + + // point X Y Z in meters + Point3 p(0, 0, 5); + StereoPoint2 result = stereoCam.project(p); + CHECK(assert_equal(StereoPoint2(320,320-150,240),result)); + + // point X Y Z in meters + Point3 p2(1, 1, 5); + StereoPoint2 result2 = stereoCam.project(p2); + CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result2)); + + // move forward 1 meter and try the same thing + // point X Y Z in meters + Point3 p3(1, 1, 6); + Rot3 unit = Rot3(); + Point3 one_meter_z(0, 0, 1); + Pose3 camPose3(unit, one_meter_z); + StereoCamera stereoCam3(camPose3, K, 0.5); + StereoPoint2 result3 = stereoCam3.project(p3); + CHECK(assert_equal(StereoPoint2(320.0+300.0, 320.0+150.0, 240.0+300),result3)); + + // camera at (0,0,1) looking right (90deg) + Point3 p4(5, 1, 0); + Rot3 right = Rot3(0, 0, 1, 0, 1, 0, -1, 0, 0); + Pose3 camPose4(right, one_meter_z); + 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): ("<(project,stereoCam, p); + Matrix actual = Dproject_stereo_pose(stereoCam, p); + CHECK(assert_equal(expected,actual,1e-7)); +} + +/* ************************************************************************* */ +TEST( StereoCamera, Dproject_stereo_point) +{ + Matrix expected = numericalDerivative22(project,stereoCam, p); + Matrix actual = Dproject_stereo_point(stereoCam, p); + CHECK(assert_equal(expected,actual,1e-8)); +} + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */