C++ (Cpp) landmarkState Examples

Example #1

File: MeasurementModel_XY.cpp Project: RangerKD/RFS-SLAM

bool MeasurementModel_XY::measure(const Pose2d &pose, 
				      const Landmark2d &landmark, 
				      Measurement2d &measurement,
				      Eigen::Matrix2d *jacobian_wrt_lmk,
				      Eigen::Matrix<double, 2, 3> *jacobian_wrt_pose){
  
  Eigen::Vector3d robotPose;
  Eigen::Vector2d mean, landmarkState;
  Eigen::Matrix2d H_lmk, landmarkUncertainty, cov;
  Eigen::Matrix<double, 2, 3> H_robot;
  Eigen::Matrix3d robotUncertainty;
  double dx_I, dy_I, dx_R, dy_R, c_RI, s_RI, range, range2, bearing;

  pose.get(robotPose, robotUncertainty);
  landmark.get(landmarkState,landmarkUncertainty);

  dx_I = landmarkState(0) - robotPose(0);
  dy_I = landmarkState(1) - robotPose(1);
  c_RI = cos(robotPose(2));
  s_RI = sin(robotPose(2));

  range2 = pow(dx_I, 2) + pow(dy_I, 2) ;
  range = sqrt(range2);

  dx_R =  c_RI * dx_I + s_RI * dy_I;
  dy_R = -s_RI * dx_I + c_RI * dy_I;
  mean << dx_R, dy_R ;

  H_lmk <<  c_RI, s_RI,
           -s_RI, c_RI;

  H_robot << -c_RI, -s_RI, -dx_I*s_RI + dy_I*c_RI,
              s_RI, -c_RI, -dx_I*c_RI - dy_I*s_RI;

  cov = H_lmk * landmarkUncertainty * H_lmk.transpose() + H_robot * robotUncertainty * H_robot.transpose() + R_;
  measurement.set(mean, cov);

  if(jacobian_wrt_lmk != NULL)
    *jacobian_wrt_lmk = H_lmk;

  if(jacobian_wrt_pose != NULL)
    *jacobian_wrt_pose = H_robot;

  if(range > config.rangeLimMax_ || range < config.rangeLimMin_)
    return false;
  else
    return true;
}

Example #2

File: MeasurementModel_RngBrg.cpp Project: bigjun/RFS-SLAM

bool MeasurementModel_RngBrg::measure(const Pose2d &pose, 
				const Landmark2d &landmark, 
				Measurement2d &measurement, 
				Eigen::Matrix2d *jacobian){

  Eigen::Vector3d robotPose;
  Eigen::Vector2d mean, landmarkState;
  Eigen::Matrix2d H, landmarkUncertainty, cov;
  double range, bearing;

  pose.get(robotPose);
  landmark.get(landmarkState,landmarkUncertainty);

  range = sqrt(  pow(landmarkState(0) - robotPose(0), 2)
		+pow(landmarkState(1) - robotPose(1), 2) );

  bearing = atan2( landmarkState(1) - robotPose(1) , landmarkState(0) - robotPose(0) ) - robotPose(2);

  while(bearing>PI) bearing-=2*PI;
  while(bearing<-PI) bearing+=2*PI;

  mean << range, bearing ;

  H <<  (landmarkState(0)-robotPose(0))/mean(0)          , (landmarkState(1)-robotPose(1))/mean(0) ,
        -(landmarkState(1)-robotPose(1))/(pow(mean(0),2)), (landmarkState(0)-robotPose(0))/pow(mean(0),2) ;
  
  cov = H * landmarkUncertainty * H.transpose() + R_;
  measurement.set(mean, cov);

  
  if(jacobian != NULL)
    *jacobian = H;

  if(range > config.rangeLimMax_ || range < config.rangeLimMin_)
    return false;
  else
    return true;
}