[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/tag measurements.h
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag/tag measurements.h |
Date: |
Wed, 14 Feb 2007 15:31:28 +0000 |
CVSROOT: /cvsroot/toon
Module name: tag
Changes by: Gerhard Reitmayr <gerhard> 07/02/14 15:31:28
Modified files:
tag : measurements.h
Log message:
corrected WorldPose and WorldPosition measurements
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/measurements.h?cvsroot=toon&r1=1.3&r2=1.4
Patches:
Index: measurements.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/measurements.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- measurements.h 5 Jun 2006 15:19:30 -0000 1.3
+++ measurements.h 14 Feb 2007 15:31:28 -0000 1.4
@@ -69,12 +69,13 @@
return jacobian;
}
- inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State &
state ){
- /// @todo need to transform covariance, see Georg paper
- return covariance;
+ inline const TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const
State & state ) const {
+ TooN::Matrix<M_DIMENSION> localCovariance = covariance;
+ state.pose.adjoint(localCovariance);
+ return localCovariance;
}
- inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
+ inline const TooN::Vector<M_DIMENSION> & getInnovation( const State &
state ) const {
return (measurement * state.pose.inverse()).ln();
}
};
@@ -98,17 +99,22 @@
TooN::Identity(jacobian.template slice<0,0,3,3>());
}
- inline TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ){
+ inline const TooN::Matrix<M_DIMENSION,State::STATE_DIMENSION> &
getMeasurementJacobian( const State & state ) const {
return jacobian;
}
- inline TooN::Matrix<M_DIMENSION> & getMeasurementCovariance( const State &
state ){
- /// @todo need to transform covariance here!
- return covariance;
+ inline const TooN::Matrix<M_DIMENSION> getMeasurementCovariance( const
State & state ) const {
+ TooN::Matrix<2 * M_DIMENSION> localCovariance;
+ TooN::Zero(localCovariance);
+ localCovariance.template slice<0,0,3,3>() = covariance;
+ state.pose.adjoint(localCovariance);
+ return localCovariance.template slice<0,0,3,3>();
}
- inline TooN::Vector<M_DIMENSION> & getInnovation( const State & state ){
- return (state.pose * position - state.pose.get_translation());
+ inline const TooN::Vector<M_DIMENSION> getInnovation( const State & state
) const {
+ /// the negative vector corresponds to the left transformation to get
from the current reference
+ /// frame to the new reference frame where position is the origin.
+ return -(state.pose * position);
}
inline void setCovariance( double sigma ){
@@ -116,7 +122,8 @@
}
inline void setCovariance( const TooN::Vector<M_DIMENSION> & sigma ){
- for(unsigned int i = 0; i < M_DIMENSION; i++){
+ TooN::Zero(covariance);
+ for(int i = 0; i < M_DIMENSION; ++i){
covariance(i,i) = sigma[i];
}
}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/tag measurements.h,
Gerhard Reitmayr <=