Fix relative velocity (and radial speed) calculation (#267)

* Fix relative velocity calculation

* Remove incorrect validation check in Ros2PublishPointVelocityMarkersNode

Author: msz-rai

extensions/ros2/src/graph/Ros2PublishPointVelocityMarkersNode.cpp

@@ -34,11 +34,6 @@ void Ros2PublishPointVelocityMarkersNode::ros2ValidateImpl()
 	if (!input->isDense()) {
 		throw InvalidPipeline(fmt::format("{} requires a compacted point cloud (dense)", getName()));
 	}
-
-	if (!input->hasField(RGL_FIELD_DYNAMIC_FORMAT)) {
-		auto msg = fmt::format("{} requires a formatted point cloud", getName());
-		throw InvalidPipeline(msg);
-	}
 }
 void Ros2PublishPointVelocityMarkersNode::ros2EnqueueExecImpl()
 {

src/gpu/optixPrograms.cu

@@ -264,11 +264,10 @@ extern "C" __global__ void __closesthit__()
 		Vec3f absPointVelocityInSensorFrame = ctx.rayOriginToWorld.rotation().inverse() * absPointVelocity;
 		Vec3f relPointVelocityBasedOnSensorLinearVelocity = absPointVelocityInSensorFrame - ctx.sensorLinearVelocityXYZ;
 
-		Vec3f distanceOnAxisXYZ = hitWorld - origin;
-		Vec3f relPointVelocityBasedOnSensorAngularVelocity = Vec3f(.0f) - ctx.sensorAngularVelocityRPY.cross(distanceOnAxisXYZ);
+		Vec3f hitRays = ctx.rayOriginToWorld.inverse() * hitWorld;
+		Vec3f relPointVelocityBasedOnSensorAngularVelocity = Vec3f(.0f) - ctx.sensorAngularVelocityRPY.cross(hitRays);
 		relPointVelocity = relPointVelocityBasedOnSensorLinearVelocity + relPointVelocityBasedOnSensorAngularVelocity;
 
-		Vec3f hitRays = ctx.rayOriginToWorld.inverse() * hitWorld;
 		radialSpeed = hitRays.normalized().dot(relPointVelocity);
 	}