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

moved StereoFactor typedef into visualSLAM namespace. StereoCamera improvements

release/4.3a0
Chris Beall 2011-03-11 21:56:14 +00:00
parent 2ce69a7250
commit 9a9b642d5d
4 changed files with 51 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/StereoCamera.cpp
View File

@ -29,12 +29,12 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) :
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> Dproject_stereo_pose,
boost::optional<Matrix&> Dproject_stereo_point) const {
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point3 cameraPoint = leftCamPose_.transform_to(point);
if (Dproject_stereo_pose) {
if (H1) {
Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose,
boost::none);
@ -43,10 +43,10 @@ StereoPoint2 StereoCamera::project(const Point3& point,
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3
*Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose;
*H1 = D_projection_intrinsic * D_intrinsic_pose;
}
if (Dproject_stereo_point) {
if (H2) {
Matrix D_cameraPoint_point;
Point3 cameraPoint = pose().transform_to(point, boost::none,
D_cameraPoint_point);
@ -55,7 +55,7 @@ StereoPoint2 StereoCamera::project(const Point3& point,
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3
*Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point;
*H2 = D_projection_intrinsic * D_intrinsic_point;
}
double d = 1.0 / cameraPoint.z();

53
gtsam/geometry/StereoCamera.h
View File

@ -17,10 +17,10 @@
#pragma once
#include <gtsam/base/Lie.h>
#include "boost/tuple/tuple.hpp"
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/geometry/StereoPoint2.h>
namespace gtsam {
@ -52,9 +52,33 @@ namespace gtsam {
return K_.baseline();
}
/*
* project 3D point and compute optional derivatives
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> Dproject_stereo_pose = boost::none,
boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/*
* to accomodate tsam's assumption that K is estimated, too
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> H2) const {
return project(point, H1, H2);
}
/*
* backproject using left camera calibration, up to scale only
* i.e. does not rely on baseline
*/
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
return pose().transform_from(cameraPoint);
}
/** Dimensionality of the tangent space */
inline size_t dim() const {
@ -63,22 +87,22 @@ namespace gtsam {
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d),calibration());
return StereoCamera(pose().expmap(d), calibration());
}
Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
return v1;
const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
return v1;
}
bool equals (const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) &&
K_.equals(camera.K_, tol);
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals(
camera.K_, tol);
}
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.") ;
K_.print(s + ".calibration.") ;
leftCamPose_.print(s + ".camera.");
K_.print(s + ".calibration.");
}
private:
@ -86,6 +110,13 @@ namespace gtsam {
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
static Matrix Duncalibrate2(const Cal3_S2& K);
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
}

7
gtsam/slam/StereoFactor.h
View File

@ -18,15 +18,12 @@
#pragma once
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
using namespace gtsam;
template<class VALUES=visualSLAM::Values, class KEY1=visualSLAM::PoseKey, class KEY2=visualSLAM::PointKey>
template<class VALUES, class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
private:
@ -114,7 +111,5 @@ private:
}
};
// Typedef for general use
typedef GenericStereoFactor<visualSLAM::Values, visualSLAM::PoseKey, visualSLAM::PointKey> StereoFactor;
}

2
gtsam/slam/visualSLAM.h
View File

@ -27,6 +27,7 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/StereoFactor.h>
namespace gtsam {
@ -126,6 +127,7 @@ namespace gtsam {
// Typedef for general use
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
/**
* Non-linear factor graph for vanilla visual SLAM