Variance Propagation errors

[jwag]

First of all, Thank you for releasing this package along with the papers. They have helped me in my own research significantly.

I have been trying to work through the error propagation math so that I can make some modifications to the elevation_mapping_cupy package including better handling of uncertainty propagation.
I have gotten a bit stumped on the step where partial derivative projected point coordinates with respect to the map-to-sensor frame. This is used in projecting the covariance of the base orientation with respect to the map (although only yaw uncertainty is included in this step). Both papers [1,2] cite another paper [3] in justification for defining this derivative using the below formula which includes a skew-symmetric matrix of a vector operation.

I admit that I have had a little trouble following all of the derivations in [3], but something appears wrong to me. My understanding is that this is the relevant formula

which includes a negative sign and applies the skew-symmetric operation to the product of the rotation and the vector not just the vector. I think the negative sign may be related to the fact that the derivative in the first equation is with respect to a transposed version of the rotation, but these two still do not agree.

Additionally, it isn't entirely clear from [3], but the provided identity seems to take the derivative of the rotation operation applied to a vector with respect to a rotation vector representation. In Appendix I section 3. it says the derivative w.r.t. the orientation itself. To me this means that a parameterization must chosen (quaternion, rotation vector, euler angles, etc.).

I have looked through the code to see if this would clarify anything and what I believe I have found is that the uncertainty of the base orientation with respect to the map is represented as a covariance matrix of Fixed-axis defined Euler angles as specified in REP 103.
Relevant section of code:

elevation_mapping/elevation_mapping/src/ElevationMapping.cpp

Lines 354 to 378 in 82aa8a5

	// Get robot pose covariance matrix at timestamp of point cloud.
	Eigen::Matrix<double, 6, 6> robotPoseCovariance;
	robotPoseCovariance.setZero();
	if (!parameters.ignoreRobotMotionUpdates_) {
	boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped const> poseMessage =
	robotPoseCache_.getElemBeforeTime(lastPointCloudUpdateTime_);
	if (!poseMessage) {
	// Tell the user that either for the timestamp no pose is available or that the buffer is possibly empty
	if (robotPoseCache_.getOldestTime().toSec() > lastPointCloudUpdateTime_.toSec()) {
	ROS_ERROR("The oldest pose available is at %f, requested pose at %f", robotPoseCache_.getOldestTime().toSec(),
	lastPointCloudUpdateTime_.toSec());
	} else {
	ROS_ERROR("Could not get pose information from robot for time %f. Buffer empty?", lastPointCloudUpdateTime_.toSec());
	}
	return;
	}
	robotPoseCovariance = Eigen::Map<const Eigen::MatrixXd>(poseMessage->pose.covariance.data(), 6, 6);
	}
	// Process point cloud.
	PointCloudType::Ptr pointCloudProcessed(new PointCloudType);
	Eigen::VectorXf measurementVariances;
	if (!sensorProcessor_->process(pointCloud, robotPoseCovariance, pointCloudProcessed, measurementVariances,
	pointCloudMsg->header.frame_id)) {
	if (!sensorProcessor_->isTfAvailableInBuffer()) {

This is passed along through the relevant sensorProcessor and then the bottom 3x3 portion is treated as $\Sigma_{P,q}$.

elevation_mapping/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp

Lines 60 to 61 in 82aa8a5

	// Robot rotation covariance matrix (Sigma_q).
	const Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast<float>();

I have attempted to validate the derivative math numerically and have found that both expressions appear to be incorrect if $\Sigma_{P,q}$ is with respect to fixed axis Euler angles applied in the order of Roll, Pitch, and then Yaw.
I have found an alternative method outlined by Lucas that I have been able to validate against the numerical derivative [4].

Here is the source code for the validation script test_rot_derivative.py.

@pfankhauser If you have the time I would appreciate you taking a look at this to see if I am understanding this properly. If so, I think a PR may be required to correct the propagation of rotational uncertainties in the sensorProcessors and possibly in the map update step as well (although I haven't looked into that yet).

References:
[1] "Robot-centric elevation mapping with uncertainty estimates" Peter Fankhauser, et. al. 2014
[2] "Elevation Mapping for Locomotion and Navigation using GPU" Takahiro Miki et. al. 2022
[3] "A Primer on the Differential Calculus of 3D Orientations" Bloesch et. al. 2016
[4] "Differentiation of the Orientation Matrix by Matrix Multipliers" Lucas 1963

[jwag]

FYI, I have modified the test_rot_derivative.py script to correct for a sign error in [4]. I also added visualization of the differences in propagated error distributions.

Here is an example where they are significantly different. Blue and green are on top of each other and represent the propagation using the numerical derivative and the Lucas method respectively. Black represents the Fankauser method and Pink represents the Bloesch formula.