C++ (Cpp) Measurement2d::get示例

示例#1

文件： MeasurementModel_RngBrg.cpp 项目： bigjun/RFS-SLAM

void MeasurementModel_RngBrg::inverseMeasure(const Pose2d &pose, 
				       const Measurement2d &measurement, 
				       Landmark2d &landmark){
  Eigen::Vector3d poseState;
  Eigen::Vector2d measurementState, mean;
  Eigen::Matrix2d measurementUncertainty, covariance, Hinv;
 
  pose.get(poseState);
  measurement.get(measurementState);
  this->getNoise(measurementUncertainty); 
  mean << poseState(0) + measurementState(0) *cos( poseState(2) + measurementState(1) ),
          poseState(1) + measurementState(0) *sin( poseState(2) + measurementState(1) );

  Hinv << cos(poseState(2)+measurementState(1)) , -measurementState(0)*sin(poseState(2)+measurementState(1)) ,
          sin(poseState(2)+measurementState(1)) , measurementState(0)*cos(poseState(2)+measurementState(1));

  covariance = Hinv * measurementUncertainty *Hinv.transpose();
  landmark.set( mean, covariance );

}

示例#2

文件： MeasurementModel_XY.cpp 项目： RangerKD/RFS-SLAM

void MeasurementModel_XY::inverseMeasure(const Pose2d &pose, 
					 const Measurement2d &measurement, 
					 Landmark2d &landmark){
  Eigen::Vector3d poseState;
  Eigen::Vector2d measurementState, mean;
  Eigen::Matrix2d measurementUncertainty, covariance, Hinv;
 
  pose.get(poseState);
  measurement.get(measurementState);
  this->getNoise(measurementUncertainty); 

  double c_RI = cos(poseState(2));
  double s_RI = sin(poseState(2));

  mean << poseState(0) + c_RI * measurementState(0) - s_RI * measurementState(1),
          poseState(1) + s_RI * measurementState(0) + c_RI * measurementState(1);

  Hinv << c_RI, -s_RI,
          s_RI, c_RI;
  
  covariance = Hinv * measurementUncertainty * Hinv.transpose();
  landmark.set( mean, covariance );

}