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

moved Stereo camera and StereoPoint to gtsam

release/4.3a0
Frank Dellaert 2010-08-08 20:23:38 +00:00
parent 62c63f9452
commit 6694b395c5
6 changed files with 409 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
geometry/Makefile.am
View File

@ -14,6 +14,10 @@ check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoin
sources += Cal3_S2.cpp CalibratedCamera.cpp SimpleCamera.cpp sources += Cal3_S2.cpp CalibratedCamera.cpp SimpleCamera.cpp
check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera check_PROGRAMS += tests/testCal3_S2 tests/testCalibratedCamera tests/testSimpleCamera
# Stereo
sources += StereoPoint2.cpp StereoCamera.cpp
check_PROGRAMS += tests/testStereoCamera
# Tensors # Tensors
headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h
headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h

128
geometry/StereoCamera.cpp Normal file
View File

@ -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;
}
*/
}

71
geometry/StereoCamera.h Normal file
View File

@ -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 <Cal3_S2.h>
#include <Pose3.h>
#include <Lie.h>
#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<StereoCamera>(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);
}

9
geometry/StereoPoint2.cpp Normal file
View File

@ -0,0 +1,9 @@
/*
* StereoPoint2.cpp
*
* Created on: Jan 26, 2010
* Author: dellaert
*/
#include "StereoPoint2.h"

101
geometry/StereoPoint2.h Normal file
View File

@ -0,0 +1,101 @@
/*
* StereoPoint2.h
*
* Created on: Jan 26, 2010
* Author: dellaert
*/
#ifndef STEREOPOINT2_H_
#define STEREOPOINT2_H_
#include <iostream>
#include <Vector.h>
#include <Testable.h>
#include <Lie.h>
#include <Point2.h>
namespace gtsam {
/**
* A 2D stereo point, v will be same for rectified images
*/
class StereoPoint2: Testable<StereoPoint2>, Lie<StereoPoint2> {
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_ */

96
geometry/tests/testStereoCamera.cpp Normal file
View File

@ -0,0 +1,96 @@
/**
* @file testStereoCamera.cpp
* @brief Unit test for StereoCamera
* single camera
* @author Chris Beall
*/
#include <CppUnitLite/TestHarness.h>
#include <numericalDerivative.h>
#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): ("<<result4(0)<<","<<result4(1)<<","<<result4(2)<<")" << endl;
}
/* ************************************************************************* */
Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
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);
/* ************************************************************************* */
TEST( StereoCamera, Dproject_stereo_pose)
{
Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(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<StereoPoint2,StereoCamera,Point3>(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);}
/* ************************************************************************* */