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

refactored StereoCamera and StereoFactor

release/4.3a0
Chris Beall 2011-03-11 17:53:16 +00:00
parent e55aafd9bc
commit 38922a38ac
7 changed files with 103 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/geometry/Cal3_S2Stereo.h Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3_S2Stereo.h
* @brief The most common 5DOF 3D->2D calibration + Stereo baseline
* @author Chris Beall
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <math.h>
namespace gtsam {
/** The most common 5DOF 3D->2D calibration */
class Cal3_S2Stereo: public Cal3_S2 {
private:
double b_;
public:
/**
* default calibration leaves coordinates unchanged
*/
Cal3_S2Stereo() :
Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {
}
/**
* constructor from doubles
*/
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
Cal3_S2(fx, fy, s, u0, v0), b_(b) {
}
void print(const std::string& s = "") const {
gtsam::print(matrix(), s);
std::cout << "Baseline: " << b_ << std::endl;
}
// /**
// * Check if equal up to specified tolerance
// */
// bool equals(const Cal3_S2& K, double tol = 10e-9) const;
inline double baseline() const { return b_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(b_);
}
};
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptrKStereo;
}

2
gtsam/geometry/Makefile.am
View File

@ -15,7 +15,7 @@ sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3
# Cameras
headers += GeneralCameraT.h
headers += GeneralCameraT.h Cal3_S2Stereo.h
sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp
check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera

18
gtsam/geometry/StereoCamera.cpp
View File

@ -23,14 +23,8 @@ 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);
StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) :
leftCamPose_(leftCamPose), K_(K) {
}
/* ************************************************************************* */
@ -65,16 +59,16 @@ StereoPoint2 StereoCamera::project(const Point3& 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();
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() - 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);
}

16
gtsam/geometry/StereoCamera.h
View File

@ -18,7 +18,7 @@
#pragma once
#include "boost/tuple/tuple.hpp"
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/geometry/StereoPoint2.h>
@ -32,17 +32,15 @@ namespace gtsam {
private:
Pose3 leftCamPose_;
Cal3_S2 K_;
double baseline_;
double fx_, fy_;
double cx_, cy_;
Cal3_S2Stereo K_;
public:
StereoCamera() {
}
StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double baseline);
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K);
const Cal3_S2& K() const {
const Cal3_S2Stereo& K() const {
return K_;
}
@ -51,7 +49,7 @@ namespace gtsam {
}
const double baseline() const {
return baseline_;
return K_.baseline();
}
StereoPoint2 project(const Point3& point,
@ -65,7 +63,7 @@ namespace gtsam {
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d),K(),baseline());
return StereoCamera(pose().expmap(d),K());
}
Vector logmap(const StereoCamera &camera) const {

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

@ -36,8 +36,8 @@ 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_S2 K(1500, 1500, 0, 320, 240);
StereoCamera stereoCam(Pose3(), K, 0.5);
Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);
@ -55,7 +55,7 @@ TEST( StereoCamera, project)
Rot3 unit = Rot3();
Point3 one_meter_z(0, 0, 1);
Pose3 camPose3(unit, one_meter_z);
StereoCamera stereoCam3(camPose3, K, 0.5);
StereoCamera stereoCam3(camPose3, K);
StereoPoint2 result3 = stereoCam3.project(p3);
CHECK(assert_equal(StereoPoint2(320.0+300.0, 320.0+150.0, 240.0+300),result3));
@ -63,7 +63,7 @@ TEST( StereoCamera, project)
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);
StereoCamera stereoCam4(camPose4, K);
StereoPoint2 result4 = stereoCam4.project(p4);
CHECK(assert_equal(StereoPoint2(320.0+300.0,320.0+150.0,240.0+300),result4));
}
@ -77,8 +77,8 @@ Pose3 camera1(Matrix_(3,3,
),
Point3(0,0,6.25));
Cal3_S2 K(1500, 1500, 0, 320, 240);
StereoCamera stereoCam(Pose3(), K, 0.5);
Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);

14
gtsam/slam/StereoFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
@ -32,8 +32,7 @@ private:
// Keep a copy of measurement and calibration for I/O
StereoPoint2 z_;
boost::shared_ptr<Cal3_S2> K_;
double baseline_;
boost::shared_ptr<Cal3_S2Stereo> K_;
public:
@ -48,7 +47,7 @@ public:
/**
* Default constructor
*/
GenericStereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)) {}
/**
* Constructor
@ -59,8 +58,8 @@ public:
* @param K the constant calibration
*/
GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose,
KEY2 j_landmark, const shared_ptrK& K, double baseline) :
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
KEY2 j_landmark, const shared_ptrKStereo& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
}
~GenericStereoFactor() {}
@ -94,7 +93,7 @@ public:
/** h(x)-z */
Vector evaluateError(const CamPose& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, *K_, baseline_);
StereoCamera stereoCam(pose, *K_);
return (stereoCam.project(point, H1, H2) - z_).vector();
}
@ -109,7 +108,6 @@ private:
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(baseline_);
}
};

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

@ -40,8 +40,8 @@ Pose3 camera1(Matrix_(3,3,
),
Point3(0,0,6.25));
Cal3_S2 K(1500, 1500, 0, 320, 240);
StereoCamera stereoCam(Pose3(), K, 0.5);
Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
StereoCamera stereoCam(Pose3(), K);
// point X Y Z in meters
Point3 p(0, 0, 5);
@ -50,15 +50,15 @@ static SharedGaussian sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
//Cal3_S2 K(625, 625, 0, 320, 240);
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(625, 625, 0, 320, 240));
//Cal3_S2 K(625, 625, 0, 320, 240, 0.5);
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<Graph> graph(new Graph());
graph->add(PoseConstraint(1,camera1));
StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph->add(StereoFactor(z14,sigma, 1, 1, K, 0.5));
graph->add(StereoFactor(z14,sigma, 1, 1, K));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());