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

Converted stereo factors to use stereo points and cameras; added operator<< to StereoPoint2 (STILL CAUSES LNK ERROR)

release/4.3a0
Stephen Camp 2014-06-05 09:25:06 -04:00
parent 5f56d70000
commit c13569df4c
7 changed files with 34 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -52,6 +52,11 @@ namespace gtsam {
/// constructor from vector
Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b) :
K_(fov, w, h), b_(b) {
}
/// @}
/// @name Testable
/// @{

5
gtsam/geometry/StereoPoint2.cpp
View File

@ -26,3 +26,8 @@ void StereoPoint2::print(const string& s) const {
cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const StereoPoint2& p) {
os << '(' << p.uL() << ", " << p.uR() << p.v() << ')';
return os;
}

2
gtsam/geometry/StereoPoint2.h
View File

@ -88,6 +88,8 @@ namespace gtsam {
StereoPoint2 operator-(const StereoPoint2& b) const {
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
}
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
/// @}
/// @name Manifold

1
gtsam/slam/SmartFactorBase.h
View File

@ -26,6 +26,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> // for Cheirality exception
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>

14
gtsam/slam/SmartStereoProjectionFactor.h
View File

@ -63,7 +63,7 @@ enum LinearizationMode {
* TODO: why LANDMARK parameter?
*/
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
protected:
// Some triangulation parameters
@ -93,7 +93,7 @@ protected:
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
@ -112,7 +112,7 @@ public:
/// shorthand for a pinhole camera
// TODO: Switch to stereoCamera:
typedef PinholeCamera<CALIBRATION> Camera;
typedef StereoCamera Camera;
// typedef StereoCamera Camera;
typedef std::vector<Camera> Cameras;
@ -264,9 +264,9 @@ public:
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
const StereoPoint2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
StereoPoint2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
@ -652,10 +652,10 @@ public:
// size_t i = 0;
// double overallError = 0;
// BOOST_FOREACH(const Camera& camera, cameras) {
// const Point2& zi = this->measured_.at(i);
// const StereoPoint2& zi = this->measured_.at(i);
// if (i == 0) // first pose
// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
// Point2 reprojectionError(
// StereoPoint2 reprojectionError(
// camera.projectPointAtInfinity(this->point_) - zi);
// overallError += 0.5
// * this->noise_.at(i)->distance(reprojectionError.vector());

8
gtsam/slam/SmartStereoProjectionPoseFactor.h
View File

@ -83,7 +83,7 @@ public:
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
*/
void add(const Point2 measured_i, const Key poseKey_i,
void add(const StereoPoint2 measured_i, const Key poseKey_i,
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
@ -97,7 +97,7 @@ public:
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
Base::add(measurements, poseKeys, noises);
@ -113,7 +113,7 @@ public:
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
@ -157,7 +157,7 @@ public:
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
typename Base::Camera camera(pose, *K_all_[i++]);
typename Base::Camera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;

19
gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -36,9 +36,10 @@ static bool isDebugTest = false;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w=640,h=480;
static double b = 1;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b));
static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b));
static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static double rankTol = 1.0;
@ -57,19 +58,19 @@ static Symbol x2('X', 2);
static Symbol x3('X', 3);
static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3_S2> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
void stereo_projectToMultipleCameras(
SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark,
vector<Point2>& measurements_cam){
StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark,
vector<StereoPoint2>& measurements_cam){
Point2 cam1_uv1 = cam1.project(landmark);
Point2 cam2_uv1 = cam2.project(landmark);
Point2 cam3_uv1 = cam3.project(landmark);
StereoPoint2 cam1_uv1 = cam1.project(landmark);
StereoPoint2 cam2_uv1 = cam2.project(landmark);
StereoPoint2 cam3_uv1 = cam3.project(landmark);
measurements_cam.push_back(cam1_uv1);
measurements_cam.push_back(cam2_uv1);
measurements_cam.push_back(cam3_uv1);