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

Generalized range factor to allow for non-planar domains

release/4.3a0
Alex Cunningham 2011-03-29 19:49:10 +00:00
parent d5ad2b2897
commit 21f7673acd
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/RangeFactor.h
View File

@ -16,8 +16,8 @@
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
//#include <gtsam/geometry/Pose2.h>
//#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -46,7 +46,7 @@ namespace gtsam {
virtual ~RangeFactor() {}
/** h(x)-z */
Vector evaluateError(const Pose2& pose, const Point2& point,
Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.range(point, H1, H2);
return Vector_(1, hx - z_);