Dereferencing boost::none in SmartFactorBase when using body_P_sensor_

Hi,

I noticed that because of a typo, there can be a segfault error in SmartFactorBase.cpp when using body_P_sensor_.

    Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
    if(body_P_sensor_){
      for(size_t i=0; i < Fs->size(); i++){
        Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
        Matrix J(6, 6);
        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
        Fs->at(i) = Fs->at(i) * J;
      }
    }

should be:

    Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
    if(Fs && body_P_sensor_){
      for(size_t i=0; i < Fs->size(); i++){
        Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
        Matrix J(6, 6);
        Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
        Fs->at(i) = Fs->at(i) * J;
      }
    }

Comments (2)