moved Stereo camera and StereoPoint to gtsam
Frank Dellaert
parent
62c63f9452
commit
6694b395c5
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
/*
|
||||
* StereoPoint2.cpp
|
||||
*
|
||||
* Created on: Jan 26, 2010
|
||||
* Author: dellaert
|
||||
*/
|
||||
|
||||
#include "StereoPoint2.h"
|
||||
|
||||
|
|
@ -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_ */
|
||||
|
|
@ -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);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue