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

StereoFactor improvements

release/4.3a0
Chris Beall 2011-03-11 06:38:50 +00:00
parent cc3db4f918
commit e55aafd9bc
2 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/StereoCamera.h
View File

@ -68,6 +68,21 @@ namespace gtsam {
return StereoCamera(pose().expmap(d),K(),baseline());
}
Vector logmap(const StereoCamera &camera) const {
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);
}
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.") ;
K_.print(s + ".calibration.") ;
}
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;

6
gtsam/slam/StereoFactor.h
View File

@ -43,6 +43,8 @@ public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
typedef typename KEY1::Value CamPose;
/**
* Default constructor
*/
@ -61,7 +63,7 @@ public:
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
}
virtual ~GenericStereoFactor() {}
~GenericStereoFactor() {}
/**
* print
@ -90,7 +92,7 @@ public:
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
Vector evaluateError(const CamPose& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, *K_, baseline_);
return (stereoCam.project(point, H1, H2) - z_).vector();