mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-11 22:00:20 +08:00
matlabbe
61edb4ee85
* Initial tests * more tests * More in-depth deskew() testing * slightly less verbose clamping corruption warning * added tf buffer related tests * added remaining tests * Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp * cleanup doc * fixing ci * rtabmap_util tests and doc * Added db_player tests * Added MapsManager tests * Added map_assembler tests * Documenting node first draft * relative links * Fixed british->usa english style. Reviewed all md files. * added link to install ros1 * updated badges * added Iron * added ubuntu * added codecov * updated coverage ci * fixing rosdep * updated ci cov job * ci bump * fixing cov ci * small doc cleanup
3508 lines
123 KiB
C++
3508 lines
123 KiB
C++
/*
|
|
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the Universite de Sherbrooke nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <limits>
|
|
|
|
#include <rtabmap_conversions/MsgConversion.h>
|
|
|
|
#include <rtabmap/core/Transform.h>
|
|
#include <rtabmap/core/util3d_transforms.h>
|
|
#include <rtabmap/core/Compression.h>
|
|
|
|
using namespace rtabmap_conversions;
|
|
|
|
namespace {
|
|
|
|
// A transform with translation and rotation on all three axes, so that a
|
|
// round-trip that drops or swaps a component cannot pass by accident.
|
|
rtabmap::Transform sampleTransform()
|
|
{
|
|
return rtabmap::Transform(1.0f, -2.0f, 3.0f, 0.1f, -0.2f, 0.3f);
|
|
}
|
|
|
|
void expectTransformNear(
|
|
const rtabmap::Transform & actual,
|
|
const rtabmap::Transform & expected,
|
|
float epsilon = 1e-5f)
|
|
{
|
|
ASSERT_FALSE(actual.isNull()) << "expected " << expected.prettyPrint();
|
|
for(int i=0; i<12; ++i)
|
|
{
|
|
EXPECT_NEAR(actual.data()[i], expected.data()[i], epsilon)
|
|
<< "at index " << i
|
|
<< "\n actual: " << actual.prettyPrint()
|
|
<< "\n expected: " << expected.prettyPrint();
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
/////////////////////////
|
|
// Transform <-> geometry_msgs
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, transformGeometryMsgRoundTrip)
|
|
{
|
|
const rtabmap::Transform in = sampleTransform();
|
|
|
|
geometry_msgs::msg::Transform msg;
|
|
transformToGeometryMsg(in, msg);
|
|
|
|
expectTransformNear(transformFromGeometryMsg(msg), in);
|
|
}
|
|
|
|
TEST(MsgConversion, transformGeometryMsgQuaternionIsNormalized)
|
|
{
|
|
geometry_msgs::msg::Transform msg;
|
|
transformToGeometryMsg(sampleTransform(), msg);
|
|
|
|
const double norm = std::sqrt(
|
|
msg.rotation.x * msg.rotation.x +
|
|
msg.rotation.y * msg.rotation.y +
|
|
msg.rotation.z * msg.rotation.z +
|
|
msg.rotation.w * msg.rotation.w);
|
|
EXPECT_NEAR(norm, 1.0, 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, transformGeometryMsgNullRoundTrip)
|
|
{
|
|
geometry_msgs::msg::Transform msg;
|
|
transformToGeometryMsg(rtabmap::Transform(), msg);
|
|
|
|
// A null transform is encoded as an all-zero quaternion.
|
|
EXPECT_EQ(msg.rotation.x, 0.0);
|
|
EXPECT_EQ(msg.rotation.y, 0.0);
|
|
EXPECT_EQ(msg.rotation.z, 0.0);
|
|
EXPECT_EQ(msg.rotation.w, 0.0);
|
|
EXPECT_TRUE(transformFromGeometryMsg(msg).isNull());
|
|
}
|
|
|
|
TEST(MsgConversion, transformGeometryMsgIdentityIsNotNull)
|
|
{
|
|
geometry_msgs::msg::Transform msg;
|
|
transformToGeometryMsg(rtabmap::Transform::getIdentity(), msg);
|
|
|
|
const rtabmap::Transform out = transformFromGeometryMsg(msg);
|
|
EXPECT_FALSE(out.isNull());
|
|
EXPECT_TRUE(out.isIdentity());
|
|
}
|
|
|
|
/////////////////////////
|
|
// Transform <-> tf2
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, transformTFRoundTrip)
|
|
{
|
|
const rtabmap::Transform in = sampleTransform();
|
|
|
|
tf2::Transform tf;
|
|
EXPECT_TRUE(transformToTF(in, tf));
|
|
|
|
expectTransformNear(transformFromTF(tf), in);
|
|
}
|
|
|
|
TEST(MsgConversion, transformTFIdentityRoundTrip)
|
|
{
|
|
tf2::Transform tf;
|
|
EXPECT_TRUE(transformToTF(rtabmap::Transform::getIdentity(), tf))
|
|
<< "an identity transform is not a null transform";
|
|
|
|
const rtabmap::Transform out = transformFromTF(tf);
|
|
EXPECT_FALSE(out.isNull());
|
|
EXPECT_TRUE(out.isIdentity());
|
|
}
|
|
|
|
TEST(MsgConversion, transformToTFWritesTranslationAndRotation)
|
|
{
|
|
// Guards against the output being left untouched: seed it with a value that
|
|
// differs from the expected result, then check it was actually overwritten.
|
|
tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99));
|
|
EXPECT_TRUE(transformToTF(sampleTransform(), tf));
|
|
|
|
EXPECT_NEAR(tf.getOrigin().x(), 1.0, 1e-5);
|
|
EXPECT_NEAR(tf.getOrigin().y(), -2.0, 1e-5);
|
|
EXPECT_NEAR(tf.getOrigin().z(), 3.0, 1e-5);
|
|
|
|
geometry_msgs::msg::Transform expected;
|
|
transformToGeometryMsg(sampleTransform(), expected);
|
|
EXPECT_NEAR(tf.getRotation().x(), expected.rotation.x, 1e-5);
|
|
EXPECT_NEAR(tf.getRotation().y(), expected.rotation.y, 1e-5);
|
|
EXPECT_NEAR(tf.getRotation().z(), expected.rotation.z, 1e-5);
|
|
EXPECT_NEAR(tf.getRotation().w(), expected.rotation.w, 1e-5);
|
|
}
|
|
|
|
TEST(MsgConversion, transformToTFNullReturnsFalseAndNaN)
|
|
{
|
|
tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99));
|
|
|
|
EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
|
|
|
|
// tf2::Transform cannot carry a null sentinel, so the output is poisoned with NaN
|
|
// on purpose: a caller that ignores the return value must fail loudly rather than
|
|
// silently proceed with a plausible-looking identity.
|
|
for(int i=0; i<3; ++i)
|
|
{
|
|
EXPECT_TRUE(std::isnan(tf.getBasis()[i].x())) << "basis row " << i;
|
|
EXPECT_TRUE(std::isnan(tf.getBasis()[i].y())) << "basis row " << i;
|
|
EXPECT_TRUE(std::isnan(tf.getBasis()[i].z())) << "basis row " << i;
|
|
}
|
|
EXPECT_TRUE(std::isnan(tf.getOrigin().x()));
|
|
EXPECT_TRUE(std::isnan(tf.getOrigin().y()));
|
|
EXPECT_TRUE(std::isnan(tf.getOrigin().z()));
|
|
|
|
const tf2::Quaternion q = tf.getRotation();
|
|
EXPECT_TRUE(std::isnan(q.x()));
|
|
EXPECT_TRUE(std::isnan(q.y()));
|
|
EXPECT_TRUE(std::isnan(q.z()));
|
|
EXPECT_TRUE(std::isnan(q.w()));
|
|
}
|
|
|
|
TEST(MsgConversion, transformToTFNullPoisonsComposition)
|
|
{
|
|
// The point of the NaN: it propagates through downstream math instead of
|
|
// quietly producing a wrong-but-finite answer.
|
|
tf2::Transform tf;
|
|
EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
|
|
|
|
const tf2::Transform composed =
|
|
tf * tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(1, 2, 3));
|
|
|
|
EXPECT_TRUE(std::isnan(composed.getOrigin().x()));
|
|
EXPECT_TRUE(std::isnan(composed.getOrigin().y()));
|
|
EXPECT_TRUE(std::isnan(composed.getOrigin().z()));
|
|
}
|
|
|
|
TEST(MsgConversion, transformFromTFDetectsNaN)
|
|
{
|
|
const tf2Scalar nan = std::numeric_limits<tf2Scalar>::quiet_NaN();
|
|
|
|
// NaN anywhere in the rotation basis...
|
|
EXPECT_TRUE(transformFromTF(tf2::Transform(
|
|
tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan),
|
|
tf2::Vector3(0, 0, 0))).isNull()) << "NaN basis";
|
|
|
|
// ...or in the translation alone must yield a null transform.
|
|
EXPECT_TRUE(transformFromTF(tf2::Transform(
|
|
tf2::Quaternion(0, 0, 0, 1),
|
|
tf2::Vector3(nan, 0, 0))).isNull()) << "NaN origin";
|
|
}
|
|
|
|
TEST(MsgConversion, transformTFNullRoundTrip)
|
|
{
|
|
// The pair round-trips a null transform: toTF poisons with NaN and reports
|
|
// false, fromTF maps that back to null.
|
|
tf2::Transform tf;
|
|
EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
|
|
EXPECT_TRUE(transformFromTF(tf).isNull());
|
|
}
|
|
|
|
TEST(MsgConversion, transformFromTFAcceptsValidTransforms)
|
|
{
|
|
// The NaN guard must not reject legitimate values, including zeros.
|
|
EXPECT_FALSE(transformFromTF(tf2::Transform(
|
|
tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0))).isNull());
|
|
EXPECT_FALSE(transformFromTF(tf2::Transform(
|
|
tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-1, 2, -3))).isNull());
|
|
}
|
|
|
|
/////////////////////////
|
|
// Transform <-> Pose
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, transformPoseMsgRoundTrip)
|
|
{
|
|
const rtabmap::Transform in = sampleTransform();
|
|
|
|
geometry_msgs::msg::Pose msg;
|
|
transformToPoseMsg(in, msg);
|
|
|
|
expectTransformNear(transformFromPoseMsg(msg), in);
|
|
}
|
|
|
|
TEST(MsgConversion, transformPoseMsgNullIsNull)
|
|
{
|
|
geometry_msgs::msg::Pose msg;
|
|
transformToPoseMsg(rtabmap::Transform(), msg);
|
|
|
|
EXPECT_TRUE(transformFromPoseMsg(msg).isNull());
|
|
}
|
|
|
|
TEST(MsgConversion, transformPoseMsgIgnoreRotationIfNotSet)
|
|
{
|
|
// Note: geometry_msgs::msg::Quaternion defaults to w=1.0, so an "unset"
|
|
// orientation has to be zeroed explicitly to reach the branch under test.
|
|
geometry_msgs::msg::Pose msg;
|
|
msg.position.x = 1.0;
|
|
msg.position.y = 2.0;
|
|
msg.position.z = 3.0;
|
|
msg.orientation.w = 0.0;
|
|
|
|
// An all-zero orientation normally yields a null transform...
|
|
|
|
EXPECT_TRUE(transformFromPoseMsg(msg, false).isNull());
|
|
|
|
// ...but with ignoreRotationIfNotSet the translation is kept with no rotation.
|
|
expectTransformNear(
|
|
transformFromPoseMsg(msg, true),
|
|
rtabmap::Transform(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f));
|
|
}
|
|
|
|
/////////////////////////
|
|
// Points and keypoints
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, point2fRoundTrip)
|
|
{
|
|
const cv::Point2f in(1.5f, -2.5f);
|
|
|
|
rtabmap_msgs::msg::Point2f msg;
|
|
point2fToROS(in, msg);
|
|
const cv::Point2f out = point2fFromROS(msg);
|
|
|
|
EXPECT_FLOAT_EQ(out.x, in.x);
|
|
EXPECT_FLOAT_EQ(out.y, in.y);
|
|
}
|
|
|
|
TEST(MsgConversion, points2fVectorRoundTrip)
|
|
{
|
|
const std::vector<cv::Point2f> in = {{1.0f, 2.0f}, {-3.0f, 4.5f}};
|
|
|
|
std::vector<rtabmap_msgs::msg::Point2f> msg;
|
|
points2fToROS(in, msg);
|
|
const std::vector<cv::Point2f> out = points2fFromROS(msg);
|
|
|
|
ASSERT_EQ(out.size(), in.size());
|
|
for(size_t i=0; i<in.size(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out[i].x, in[i].x) << "at " << i;
|
|
EXPECT_FLOAT_EQ(out[i].y, in[i].y) << "at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, point3fRoundTrip)
|
|
{
|
|
const cv::Point3f in(1.5f, -2.5f, 3.5f);
|
|
|
|
rtabmap_msgs::msg::Point3f msg;
|
|
point3fToROS(in, msg);
|
|
const cv::Point3f out = point3fFromROS(msg);
|
|
|
|
EXPECT_FLOAT_EQ(out.x, in.x);
|
|
EXPECT_FLOAT_EQ(out.y, in.y);
|
|
EXPECT_FLOAT_EQ(out.z, in.z);
|
|
}
|
|
|
|
TEST(MsgConversion, points3fVectorRoundTripWithoutTransform)
|
|
{
|
|
const std::vector<cv::Point3f> in = {{1.0f, 2.0f, 3.0f}, {-4.0f, 5.0f, -6.0f}};
|
|
|
|
std::vector<rtabmap_msgs::msg::Point3f> msg;
|
|
points3fToROS(in, msg);
|
|
const std::vector<cv::Point3f> out = points3fFromROS(msg);
|
|
|
|
ASSERT_EQ(out.size(), in.size());
|
|
for(size_t i=0; i<in.size(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out[i].x, in[i].x) << "at " << i;
|
|
EXPECT_FLOAT_EQ(out[i].y, in[i].y) << "at " << i;
|
|
EXPECT_FLOAT_EQ(out[i].z, in[i].z) << "at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, points3fTransformIsAppliedOnBothDirections)
|
|
{
|
|
const std::vector<cv::Point3f> in = {{1.0f, 2.0f, 3.0f}};
|
|
const rtabmap::Transform t = sampleTransform();
|
|
|
|
// Applying t on the way out and t.inverse() on the way in must cancel.
|
|
std::vector<rtabmap_msgs::msg::Point3f> msg;
|
|
points3fToROS(in, msg, t);
|
|
const std::vector<cv::Point3f> out = points3fFromROS(msg, t.inverse());
|
|
|
|
ASSERT_EQ(out.size(), in.size());
|
|
EXPECT_NEAR(out[0].x, in[0].x, 1e-4);
|
|
EXPECT_NEAR(out[0].y, in[0].y, 1e-4);
|
|
EXPECT_NEAR(out[0].z, in[0].z, 1e-4);
|
|
|
|
// ...and the intermediate message really is the transformed point.
|
|
const cv::Point3f expected = rtabmap::util3d::transformPoint(in[0], t);
|
|
EXPECT_NEAR(msg[0].x, expected.x, 1e-4);
|
|
EXPECT_NEAR(msg[0].y, expected.y, 1e-4);
|
|
EXPECT_NEAR(msg[0].z, expected.z, 1e-4);
|
|
}
|
|
|
|
TEST(MsgConversion, points3fFromROSAppendsToExistingVector)
|
|
{
|
|
std::vector<rtabmap_msgs::msg::Point3f> msg(2);
|
|
msg[0].x = 1.0f;
|
|
msg[1].x = 2.0f;
|
|
|
|
std::vector<cv::Point3f> points = {{9.0f, 9.0f, 9.0f}};
|
|
points3fFromROS(msg, points);
|
|
|
|
ASSERT_EQ(points.size(), 3u);
|
|
EXPECT_FLOAT_EQ(points[0].x, 9.0f) << "existing content must be preserved";
|
|
EXPECT_FLOAT_EQ(points[1].x, 1.0f);
|
|
EXPECT_FLOAT_EQ(points[2].x, 2.0f);
|
|
}
|
|
|
|
TEST(MsgConversion, keypointRoundTrip)
|
|
{
|
|
const cv::KeyPoint in(cv::Point2f(10.0f, 20.0f), 7.0f, 45.0f, 0.5f, 2, 3);
|
|
|
|
rtabmap_msgs::msg::KeyPoint msg;
|
|
keypointToROS(in, msg);
|
|
const cv::KeyPoint out = keypointFromROS(msg);
|
|
|
|
EXPECT_FLOAT_EQ(out.pt.x, in.pt.x);
|
|
EXPECT_FLOAT_EQ(out.pt.y, in.pt.y);
|
|
EXPECT_FLOAT_EQ(out.size, in.size);
|
|
EXPECT_FLOAT_EQ(out.angle, in.angle);
|
|
EXPECT_FLOAT_EQ(out.response, in.response);
|
|
EXPECT_EQ(out.octave, in.octave);
|
|
EXPECT_EQ(out.class_id, in.class_id);
|
|
}
|
|
|
|
TEST(MsgConversion, keypointsFromROSAppendsAndAppliesXShift)
|
|
{
|
|
const std::vector<cv::KeyPoint> in = {
|
|
cv::KeyPoint(cv::Point2f(10.0f, 20.0f), 7.0f),
|
|
cv::KeyPoint(cv::Point2f(30.0f, 40.0f), 7.0f)};
|
|
|
|
std::vector<rtabmap_msgs::msg::KeyPoint> msg;
|
|
keypointsToROS(in, msg);
|
|
ASSERT_EQ(msg.size(), in.size());
|
|
|
|
std::vector<cv::KeyPoint> kpts = {cv::KeyPoint(cv::Point2f(1.0f, 1.0f), 1.0f)};
|
|
keypointsFromROS(msg, kpts, /*xShift=*/100);
|
|
|
|
ASSERT_EQ(kpts.size(), 3u);
|
|
EXPECT_FLOAT_EQ(kpts[0].pt.x, 1.0f) << "existing content must be preserved";
|
|
EXPECT_FLOAT_EQ(kpts[1].pt.x, 110.0f);
|
|
EXPECT_FLOAT_EQ(kpts[2].pt.x, 130.0f);
|
|
EXPECT_FLOAT_EQ(kpts[1].pt.y, 20.0f) << "xShift must not touch y";
|
|
}
|
|
|
|
/////////////////////////
|
|
// Timestamps
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, timestampRoundTrip)
|
|
{
|
|
// Not exact on purpose: a double resolves to a few hundred nanoseconds at this
|
|
// magnitude, so the round trip is only good to about a microsecond.
|
|
const double in = 1234567890.123456;
|
|
EXPECT_NEAR(timestampFromROS(timestampToROS(in)), in, 1e-6);
|
|
}
|
|
|
|
TEST(MsgConversion, timestampToROSUsesRosClock)
|
|
{
|
|
// Message header stamps convert to RCL_ROS_TIME, while rclcpp::Time(sec, nsec)
|
|
// defaults to RCL_SYSTEM_TIME. Comparing two different clock types throws, so a
|
|
// timestamp built here must be comparable with one taken from a message -- several
|
|
// conversions do exactly that when syncing to an odometry stamp.
|
|
const rclcpp::Time built = timestampToROS(1000.0);
|
|
EXPECT_EQ(built.get_clock_type(), RCL_ROS_TIME);
|
|
|
|
builtin_interfaces::msg::Time asMsg = timestampToROS(1000.5);
|
|
const rclcpp::Time fromMsg(asMsg);
|
|
EXPECT_EQ(fromMsg.get_clock_type(), RCL_ROS_TIME);
|
|
|
|
EXPECT_NO_THROW({ volatile bool differ = (built != fromMsg); (void)differ; })
|
|
<< "a built stamp must be comparable with a message-derived one";
|
|
}
|
|
|
|
TEST(MsgConversion, timestampZeroRoundTrip)
|
|
{
|
|
EXPECT_EQ(timestampFromROS(timestampToROS(0.0)), 0.0);
|
|
}
|
|
|
|
/////////////////////////
|
|
// sizeOfPointField
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, sizeOfPointField)
|
|
{
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT8), 1);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT8), 1);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT16), 2);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT16), 2);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT32), 4);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT32), 4);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32), 4);
|
|
EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT64), 8);
|
|
}
|
|
|
|
TEST(MsgConversion, sizeOfPointFieldThrowsOnUnknownType)
|
|
{
|
|
EXPECT_THROW(sizeOfPointField(42), std::runtime_error);
|
|
}
|
|
|
|
/////////////////////////
|
|
// getClosestIterator
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, getClosestIterator)
|
|
{
|
|
const std::map<double, int> buffer = {{1.0, 10}, {2.0, 20}, {3.0, 30}};
|
|
|
|
EXPECT_EQ(getClosestIterator(buffer, 1.0)->second, 10) << "exact match";
|
|
EXPECT_EQ(getClosestIterator(buffer, 2.0)->second, 20) << "exact match";
|
|
EXPECT_EQ(getClosestIterator(buffer, 1.4)->second, 10) << "closer to lower";
|
|
EXPECT_EQ(getClosestIterator(buffer, 1.6)->second, 20) << "closer to upper";
|
|
EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 10) << "clamped below range";
|
|
EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 30) << "clamped above range";
|
|
}
|
|
|
|
TEST(MsgConversion, getClosestIteratorSingleEntry)
|
|
{
|
|
const std::map<double, int> buffer = {{5.0, 50}};
|
|
|
|
EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 50);
|
|
EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 50);
|
|
}
|
|
|
|
/////////////////////////
|
|
// compressedMat <-> bytes
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, compressedMatRoundTrip)
|
|
{
|
|
const cv::Mat in = (cv::Mat_<unsigned char>(1, 5) << 1, 2, 3, 250, 255);
|
|
|
|
std::vector<unsigned char> bytes;
|
|
compressedMatToBytes(in, bytes);
|
|
ASSERT_EQ(bytes.size(), 5u);
|
|
|
|
const cv::Mat out = compressedMatFromBytes(bytes);
|
|
ASSERT_EQ(out.type(), CV_8UC1);
|
|
ASSERT_EQ(out.total(), in.total());
|
|
EXPECT_EQ(cv::countNonZero(out.reshape(1, 1) != in.reshape(1, 1)), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, compressedMatEmptyRoundTrip)
|
|
{
|
|
std::vector<unsigned char> bytes = {1, 2, 3};
|
|
compressedMatToBytes(cv::Mat(), bytes);
|
|
|
|
EXPECT_TRUE(bytes.empty()) << "output must be cleared";
|
|
EXPECT_TRUE(compressedMatFromBytes(bytes).empty());
|
|
}
|
|
|
|
TEST(MsgConversion, compressedMatFromBytesCopyFlag)
|
|
{
|
|
std::vector<unsigned char> bytes = {1, 2, 3};
|
|
|
|
const cv::Mat shared = compressedMatFromBytes(bytes, /*copy=*/false);
|
|
const cv::Mat copied = compressedMatFromBytes(bytes, /*copy=*/true);
|
|
|
|
bytes[0] = 99;
|
|
EXPECT_EQ(shared.at<unsigned char>(0, 0), 99) << "copy=false must alias the input";
|
|
EXPECT_EQ(copied.at<unsigned char>(0, 0), 1) << "copy=true must be independent";
|
|
}
|
|
|
|
/////////////////////////
|
|
// EnvSensor
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, envSensorRoundTrip)
|
|
{
|
|
const rtabmap::EnvSensor in(
|
|
rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1234567890.5);
|
|
|
|
rtabmap_msgs::msg::EnvSensor msg;
|
|
envSensorToROS(in, msg);
|
|
const rtabmap::EnvSensor out = envSensorFromROS(msg);
|
|
|
|
EXPECT_EQ(out.type(), in.type());
|
|
EXPECT_DOUBLE_EQ(out.value(), in.value());
|
|
EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
|
|
}
|
|
|
|
TEST(MsgConversion, envSensorsRoundTripKeyedByType)
|
|
{
|
|
rtabmap::EnvSensors in;
|
|
in.insert(std::make_pair(
|
|
rtabmap::EnvSensor::kAmbientTemperature,
|
|
rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1.0)));
|
|
in.insert(std::make_pair(
|
|
rtabmap::EnvSensor::kAmbientLight,
|
|
rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientLight, 300.0, 2.0)));
|
|
|
|
std::vector<rtabmap_msgs::msg::EnvSensor> msg;
|
|
envSensorsToROS(in, msg);
|
|
ASSERT_EQ(msg.size(), in.size());
|
|
|
|
const rtabmap::EnvSensors out = envSensorsFromROS(msg);
|
|
ASSERT_EQ(out.size(), in.size());
|
|
for(rtabmap::EnvSensors::const_iterator iter=in.begin(); iter!=in.end(); ++iter)
|
|
{
|
|
rtabmap::EnvSensors::const_iterator found = out.find(iter->first);
|
|
ASSERT_NE(found, out.end()) << "missing type " << iter->first;
|
|
EXPECT_DOUBLE_EQ(found->second.value(), iter->second.value());
|
|
}
|
|
}
|
|
|
|
/////////////////////////
|
|
// Link
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, linkRoundTrip)
|
|
{
|
|
cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1) * 3.0;
|
|
const rtabmap::Link in(
|
|
1, 2, rtabmap::Link::kGlobalClosure, sampleTransform(), information);
|
|
|
|
rtabmap_msgs::msg::Link msg;
|
|
linkToROS(in, msg);
|
|
const rtabmap::Link out = linkFromROS(msg);
|
|
|
|
EXPECT_EQ(out.from(), in.from());
|
|
EXPECT_EQ(out.to(), in.to());
|
|
EXPECT_EQ(out.type(), in.type());
|
|
expectTransformNear(out.transform(), in.transform());
|
|
|
|
ASSERT_EQ(out.infMatrix().rows, 6);
|
|
ASSERT_EQ(out.infMatrix().cols, 6);
|
|
for(int i=0; i<6; ++i)
|
|
{
|
|
for(int j=0; j<6; ++j)
|
|
{
|
|
EXPECT_DOUBLE_EQ(
|
|
out.infMatrix().at<double>(i, j),
|
|
in.infMatrix().at<double>(i, j)) << "at " << i << "," << j;
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////
|
|
// CameraModel
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, cameraModelFromROSReadsIntrinsics)
|
|
{
|
|
sensor_msgs::msg::CameraInfo in;
|
|
in.width = 640;
|
|
in.height = 480;
|
|
in.distortion_model = "plumb_bob";
|
|
in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
|
|
in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
|
|
in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
|
|
|
const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
|
|
const rtabmap::CameraModel model = cameraModelFromROS(in, localTransform);
|
|
|
|
EXPECT_EQ(model.imageWidth(), 640);
|
|
EXPECT_EQ(model.imageHeight(), 480);
|
|
EXPECT_NEAR(model.fx(), 525.0, 1e-9);
|
|
EXPECT_NEAR(model.fy(), 525.0, 1e-9);
|
|
EXPECT_NEAR(model.cx(), 320.0, 1e-9);
|
|
EXPECT_NEAR(model.cy(), 240.0, 1e-9);
|
|
expectTransformNear(model.localTransform(), localTransform);
|
|
|
|
// The raw distortion coefficients are kept verbatim.
|
|
ASSERT_EQ(model.D_raw().cols, 5);
|
|
for(size_t i=0; i<in.d.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(model.D_raw().at<double>(0, i), in.d[i], 1e-9) << "d at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelToROSRectifiedHasNoDistortion)
|
|
{
|
|
sensor_msgs::msg::CameraInfo in;
|
|
in.width = 640;
|
|
in.height = 480;
|
|
in.distortion_model = "plumb_bob";
|
|
in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
|
|
in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
|
|
in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
|
|
|
sensor_msgs::msg::CameraInfo out;
|
|
cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out);
|
|
|
|
EXPECT_EQ(out.width, in.width);
|
|
EXPECT_EQ(out.height, in.height);
|
|
|
|
// A model carrying a projection matrix describes an already-rectified image,
|
|
// so cameraModelToROS deliberately emits zero distortion rather than echoing
|
|
// back the raw coefficients. K and P do round-trip unchanged.
|
|
EXPECT_EQ(out.distortion_model, "plumb_bob");
|
|
ASSERT_EQ(out.d.size(), 5u);
|
|
for(size_t i=0; i<out.d.size(); ++i)
|
|
{
|
|
EXPECT_DOUBLE_EQ(out.d[i], 0.0) << "d at " << i;
|
|
}
|
|
for(size_t i=0; i<in.k.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.k[i], in.k[i], 1e-9) << "k at " << i;
|
|
}
|
|
for(size_t i=0; i<in.p.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.p[i], in.p[i], 1e-9) << "p at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelFromROSPacksFisheyeDistortion)
|
|
{
|
|
// Fisheye/equidistant models carry 4 coefficients, which rtabmap stores in a
|
|
// 1x6 matrix at positions 0, 1, 4 and 5.
|
|
for(const std::string & model : {"fisheye", "equidistant", "Kannala Brandt4"})
|
|
{
|
|
sensor_msgs::msg::CameraInfo in;
|
|
in.width = 640;
|
|
in.height = 480;
|
|
in.distortion_model = model;
|
|
in.d = {0.1, 0.2, 0.3, 0.4};
|
|
in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
|
|
|
|
const cv::Mat D = cameraModelFromROS(in, rtabmap::Transform::getIdentity()).D_raw();
|
|
|
|
ASSERT_EQ(D.total(), 6u) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 0), 0.1, 1e-9) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 1), 0.2, 1e-9) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 2), 0.0, 1e-9) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 3), 0.0, 1e-9) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 4), 0.3, 1e-9) << "distortion_model=" << model;
|
|
EXPECT_NEAR(D.at<double>(0, 5), 0.4, 1e-9) << "distortion_model=" << model;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelToROSUnpacksFisheyeDistortion)
|
|
{
|
|
// Built with an empty P: cameraModelToROS only reports "equidistant" for a
|
|
// raw (unrectified) model. Note this cannot be produced by cameraModelFromROS,
|
|
// whose P is a fixed-size array and therefore never empty.
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
|
|
cv::Mat D = cv::Mat::zeros(1, 6, CV_64FC1);
|
|
D.at<double>(0, 0) = 0.1;
|
|
D.at<double>(0, 1) = 0.2;
|
|
D.at<double>(0, 4) = 0.3;
|
|
D.at<double>(0, 5) = 0.4;
|
|
|
|
const rtabmap::CameraModel model(
|
|
"fisheye", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(),
|
|
rtabmap::Transform::getIdentity());
|
|
|
|
sensor_msgs::msg::CameraInfo out;
|
|
cameraModelToROS(model, out);
|
|
|
|
EXPECT_EQ(out.distortion_model, "equidistant");
|
|
ASSERT_EQ(out.d.size(), 4u);
|
|
EXPECT_NEAR(out.d[0], 0.1, 1e-9);
|
|
EXPECT_NEAR(out.d[1], 0.2, 1e-9);
|
|
EXPECT_NEAR(out.d[2], 0.3, 1e-9);
|
|
EXPECT_NEAR(out.d[3], 0.4, 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelToROSRationalPolynomialDistortion)
|
|
{
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
|
|
cv::Mat D = (cv::Mat_<double>(1, 8) <<
|
|
0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8);
|
|
|
|
const rtabmap::CameraModel model(
|
|
"rational", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(),
|
|
rtabmap::Transform::getIdentity());
|
|
|
|
sensor_msgs::msg::CameraInfo out;
|
|
cameraModelToROS(model, out);
|
|
|
|
EXPECT_EQ(out.distortion_model, "rational_polynomial");
|
|
ASSERT_EQ(out.d.size(), 8u);
|
|
for(size_t i=0; i<out.d.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.d[i], 0.1 * (i + 1), 1e-9) << "d at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelFromROSTreatsUnsetMatricesAsAbsent)
|
|
{
|
|
// k, r and p are fixed-size arrays, so "unset" means all-zero rather than empty.
|
|
sensor_msgs::msg::CameraInfo in;
|
|
in.width = 640;
|
|
in.height = 480;
|
|
in.distortion_model = "plumb_bob";
|
|
in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
|
|
in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
|
|
// r and p deliberately left at their all-zero defaults.
|
|
|
|
const rtabmap::CameraModel model =
|
|
cameraModelFromROS(in, rtabmap::Transform::getIdentity());
|
|
|
|
EXPECT_TRUE(model.P().empty()) << "an all-zero P must not become a 3x4 of zeros";
|
|
EXPECT_TRUE(model.R().empty()) << "an all-zero R must not become a 3x3 of zeros";
|
|
EXPECT_FALSE(model.K_raw().empty());
|
|
|
|
// With no P, the intrinsics must come from K instead of an all-zero P.
|
|
EXPECT_NEAR(model.fx(), 525.0, 1e-9);
|
|
EXPECT_NEAR(model.fy(), 525.0, 1e-9);
|
|
EXPECT_NEAR(model.cx(), 320.0, 1e-9);
|
|
EXPECT_NEAR(model.cy(), 240.0, 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelUnrectifiedFisheyeRoundTrip)
|
|
{
|
|
// Without a projection matrix the model stays raw, so the fisheye coefficients
|
|
// survive the full message -> model -> message round trip.
|
|
sensor_msgs::msg::CameraInfo in;
|
|
in.width = 640;
|
|
in.height = 480;
|
|
in.distortion_model = "equidistant";
|
|
in.d = {0.1, 0.2, 0.3, 0.4};
|
|
in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
|
|
|
|
sensor_msgs::msg::CameraInfo out;
|
|
cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out);
|
|
|
|
EXPECT_EQ(out.distortion_model, "equidistant");
|
|
ASSERT_EQ(out.d.size(), 4u);
|
|
for(size_t i=0; i<in.d.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.d[i], in.d[i], 1e-9) << "d at " << i;
|
|
}
|
|
for(size_t i=0; i<in.k.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.k[i], in.k[i], 1e-9) << "k at " << i;
|
|
}
|
|
|
|
// An unset R is reported as identity.
|
|
const std::array<double, 9> identity = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
|
|
for(size_t i=0; i<identity.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.r[i], identity[i], 1e-9) << "r at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, cameraModelToROSSynthesizesPFromK)
|
|
{
|
|
// With no P of its own, cameraModelToROS builds P = [K | 0].
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
|
|
|
|
const rtabmap::CameraModel model(
|
|
"raw", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(),
|
|
rtabmap::Transform::getIdentity());
|
|
|
|
sensor_msgs::msg::CameraInfo out;
|
|
cameraModelToROS(model, out);
|
|
|
|
const std::array<double, 12> expected = {
|
|
525.0, 0.0, 320.0, 0.0,
|
|
0.0, 525.0, 240.0, 0.0,
|
|
0.0, 0.0, 1.0, 0.0};
|
|
for(size_t i=0; i<expected.size(); ++i)
|
|
{
|
|
EXPECT_NEAR(out.p[i], expected[i], 1e-9) << "p at " << i;
|
|
}
|
|
// P(2,3) is a translation term and must stay zero for a single camera.
|
|
EXPECT_DOUBLE_EQ(out.p[11], 0.0);
|
|
}
|
|
|
|
/////////////////////////
|
|
// GlobalDescriptor
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, globalDescriptorRoundTrip)
|
|
{
|
|
cv::Mat data = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
|
|
cv::Mat info = (cv::Mat_<float>(1, 2) << 9.0f, 8.0f);
|
|
const rtabmap::GlobalDescriptor in(7, data, info);
|
|
|
|
rtabmap_msgs::msg::GlobalDescriptor msg;
|
|
globalDescriptorToROS(in, msg);
|
|
const rtabmap::GlobalDescriptor out = globalDescriptorFromROS(msg);
|
|
|
|
EXPECT_EQ(out.type(), in.type());
|
|
ASSERT_EQ(out.data().total(), in.data().total());
|
|
for(size_t i=0; i<in.data().total(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out.data().at<float>(0, i), in.data().at<float>(0, i)) << "data at " << i;
|
|
}
|
|
ASSERT_EQ(out.info().total(), in.info().total());
|
|
for(size_t i=0; i<in.info().total(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out.info().at<float>(0, i), in.info().at<float>(0, i)) << "info at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, globalDescriptorsVectorRoundTrip)
|
|
{
|
|
std::vector<rtabmap::GlobalDescriptor> in;
|
|
in.push_back(rtabmap::GlobalDescriptor(1, (cv::Mat_<float>(1, 2) << 1.0f, 2.0f)));
|
|
in.push_back(rtabmap::GlobalDescriptor(2, (cv::Mat_<float>(1, 2) << 3.0f, 4.0f)));
|
|
|
|
std::vector<rtabmap_msgs::msg::GlobalDescriptor> msg;
|
|
globalDescriptorsToROS(in, msg);
|
|
ASSERT_EQ(msg.size(), in.size());
|
|
|
|
const std::vector<rtabmap::GlobalDescriptor> out = globalDescriptorsFromROS(msg);
|
|
ASSERT_EQ(out.size(), in.size());
|
|
for(size_t i=0; i<in.size(); ++i)
|
|
{
|
|
EXPECT_EQ(out[i].type(), in[i].type()) << "at " << i;
|
|
EXPECT_FLOAT_EQ(out[i].data().at<float>(0, 0), in[i].data().at<float>(0, 0)) << "at " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, globalDescriptorsEmptyRoundTrip)
|
|
{
|
|
std::vector<rtabmap_msgs::msg::GlobalDescriptor> msg(3);
|
|
globalDescriptorsToROS(std::vector<rtabmap::GlobalDescriptor>(), msg);
|
|
|
|
EXPECT_TRUE(msg.empty()) << "output must be cleared";
|
|
EXPECT_TRUE(globalDescriptorsFromROS(msg).empty());
|
|
}
|
|
|
|
/////////////////////////
|
|
// UserData
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, userDataUncompressedRoundTrip)
|
|
{
|
|
const cv::Mat in = (cv::Mat_<int>(2, 3) << 1, 2, 3, 4, 5, 6);
|
|
|
|
rtabmap_msgs::msg::UserData msg;
|
|
userDataToROS(in, msg, /*compress=*/false);
|
|
|
|
EXPECT_EQ(msg.rows, in.rows);
|
|
EXPECT_EQ(msg.cols, in.cols);
|
|
EXPECT_EQ(msg.type, in.type());
|
|
|
|
const cv::Mat out = userDataFromROS(msg);
|
|
ASSERT_EQ(out.rows, in.rows);
|
|
ASSERT_EQ(out.cols, in.cols);
|
|
ASSERT_EQ(out.type(), in.type());
|
|
EXPECT_EQ(cv::countNonZero(out != in), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, userDataCompressedRoundTrip)
|
|
{
|
|
const cv::Mat in = (cv::Mat_<int>(2, 3) << 1, 2, 3, 4, 5, 6);
|
|
|
|
rtabmap_msgs::msg::UserData msg;
|
|
userDataToROS(in, msg, /*compress=*/true);
|
|
|
|
// Compressed payloads travel as a 1xN byte blob.
|
|
EXPECT_EQ(msg.rows, 1);
|
|
EXPECT_EQ(msg.type, CV_8UC1);
|
|
EXPECT_EQ((size_t)msg.cols, msg.data.size());
|
|
|
|
// userDataFromROS hands back the still-compressed blob; the caller uncompresses.
|
|
const cv::Mat blob = userDataFromROS(msg);
|
|
ASSERT_FALSE(blob.empty());
|
|
const cv::Mat out = rtabmap::uncompressData(blob);
|
|
|
|
ASSERT_EQ(out.rows, in.rows);
|
|
ASSERT_EQ(out.cols, in.cols);
|
|
ASSERT_EQ(out.type(), in.type());
|
|
EXPECT_EQ(cv::countNonZero(out != in), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, userDataEmpty)
|
|
{
|
|
rtabmap_msgs::msg::UserData msg;
|
|
userDataToROS(cv::Mat(), msg, /*compress=*/false);
|
|
EXPECT_TRUE(msg.data.empty());
|
|
EXPECT_TRUE(userDataFromROS(msg).empty());
|
|
}
|
|
|
|
/////////////////////////
|
|
// StereoCameraModel
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, stereoCameraModelFromROS)
|
|
{
|
|
const double fx = 525.0;
|
|
const double baseline = 0.12;
|
|
|
|
sensor_msgs::msg::CameraInfo left;
|
|
left.width = 640;
|
|
left.height = 480;
|
|
left.k = {fx, 0.0, 320.0, 0.0, fx, 240.0, 0.0, 0.0, 1.0};
|
|
left.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
left.p = {fx, 0.0, 320.0, 0.0, 0.0, fx, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
|
|
|
// The right camera carries the baseline in P(0,3) = -fx * baseline.
|
|
sensor_msgs::msg::CameraInfo right = left;
|
|
right.p[3] = -fx * baseline;
|
|
|
|
const rtabmap::StereoCameraModel model = stereoCameraModelFromROS(
|
|
left, right, rtabmap::Transform::getIdentity());
|
|
|
|
EXPECT_NEAR(model.left().fx(), fx, 1e-9);
|
|
EXPECT_NEAR(model.right().fx(), fx, 1e-9);
|
|
EXPECT_NEAR(model.baseline(), baseline, 1e-9);
|
|
EXPECT_TRUE(model.isValidForProjection());
|
|
}
|
|
|
|
/////////////////////////
|
|
// OdometryInfo
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, odomInfoRoundTrip)
|
|
{
|
|
rtabmap::OdometryInfo in;
|
|
in.lost = false;
|
|
in.features = 500;
|
|
in.localMapSize = 1000;
|
|
in.localScanMapSize = 2000;
|
|
in.localKeyFrames = 5;
|
|
in.keyFrameAdded = true;
|
|
in.timeEstimation = 0.02f;
|
|
in.interval = 0.033;
|
|
in.distanceTravelled = 12.5f;
|
|
in.reg.matches = 300;
|
|
in.reg.inliers = 250;
|
|
in.transform = sampleTransform();
|
|
|
|
rtabmap_msgs::msg::OdomInfo msg;
|
|
odomInfoToROS(in, msg);
|
|
const rtabmap::OdometryInfo out = odomInfoFromROS(msg);
|
|
|
|
EXPECT_EQ(out.lost, in.lost);
|
|
EXPECT_EQ(out.features, in.features);
|
|
EXPECT_EQ(out.localMapSize, in.localMapSize);
|
|
EXPECT_EQ(out.localScanMapSize, in.localScanMapSize);
|
|
EXPECT_EQ(out.localKeyFrames, in.localKeyFrames);
|
|
EXPECT_EQ(out.keyFrameAdded, in.keyFrameAdded);
|
|
EXPECT_FLOAT_EQ(out.timeEstimation, in.timeEstimation);
|
|
EXPECT_NEAR(out.interval, in.interval, 1e-6);
|
|
EXPECT_FLOAT_EQ(out.distanceTravelled, in.distanceTravelled);
|
|
EXPECT_EQ(out.reg.matches, in.reg.matches);
|
|
EXPECT_EQ(out.reg.inliers, in.reg.inliers);
|
|
expectTransformNear(out.transform, in.transform);
|
|
}
|
|
|
|
TEST(MsgConversion, odomInfoIgnoreDataDropsHeavyMembers)
|
|
{
|
|
rtabmap::OdometryInfo in;
|
|
in.features = 500;
|
|
in.reg.inliers = 250;
|
|
in.words.insert(std::make_pair(1, cv::KeyPoint(cv::Point2f(1, 2), 3)));
|
|
in.localMap.insert(std::make_pair(1, cv::Point3f(1, 2, 3)));
|
|
|
|
rtabmap_msgs::msg::OdomInfo full;
|
|
odomInfoToROS(in, full, /*ignoreData=*/false);
|
|
EXPECT_FALSE(full.words_keys.empty());
|
|
|
|
rtabmap_msgs::msg::OdomInfo light;
|
|
odomInfoToROS(in, light, /*ignoreData=*/true);
|
|
EXPECT_TRUE(light.words_keys.empty()) << "heavy members must be dropped";
|
|
|
|
// The scalar statistics survive either way.
|
|
EXPECT_EQ(odomInfoFromROS(light).features, in.features);
|
|
EXPECT_EQ(odomInfoFromROS(light).reg.inliers, in.reg.inliers);
|
|
}
|
|
|
|
TEST(MsgConversion, odomInfoToStatistics)
|
|
{
|
|
rtabmap::OdometryInfo info;
|
|
info.features = 400;
|
|
info.reg.inliers = 100;
|
|
info.reg.matches = 200;
|
|
info.localMapSize = 1234;
|
|
|
|
const std::map<std::string, float> stats = odomInfoToStatistics(info);
|
|
|
|
ASSERT_TRUE(stats.find("Odometry/Features/") != stats.end());
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/Features/"), 400.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/Matches/"), 200.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/Inliers/"), 100.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/LocalMapSize/"), 1234.0f);
|
|
// MatchesRatio is inliers/features, and must not divide by zero.
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/MatchesRatio/"), 100.0f/400.0f);
|
|
}
|
|
|
|
TEST(MsgConversion, odomInfoToStatisticsEmptyCovariance)
|
|
{
|
|
// RegistrationInfo does not initialize covariance, so a plain OdometryInfo has
|
|
// an empty matrix. Reading it must not be attempted.
|
|
rtabmap::OdometryInfo info;
|
|
ASSERT_TRUE(info.reg.covariance.empty()) << "precondition";
|
|
|
|
const std::map<std::string, float> stats = odomInfoToStatistics(info);
|
|
|
|
EXPECT_TRUE(stats.find("Odometry/StdDevLin/") == stats.end())
|
|
<< "covariance-derived stats must be omitted, not read out of bounds";
|
|
EXPECT_TRUE(stats.find("Odometry/VarianceAng/") == stats.end());
|
|
// The rest of the statistics are still produced.
|
|
EXPECT_TRUE(stats.find("Odometry/Features/") != stats.end());
|
|
}
|
|
|
|
TEST(MsgConversion, odomInfoToStatisticsWithCovariance)
|
|
{
|
|
rtabmap::OdometryInfo info;
|
|
info.reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 4.0;
|
|
|
|
const std::map<std::string, float> stats = odomInfoToStatistics(info);
|
|
|
|
ASSERT_TRUE(stats.find("Odometry/VarianceLin/") != stats.end());
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceLin/"), 4.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevLin/"), 2.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceAng/"), 4.0f);
|
|
EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevAng/"), 2.0f);
|
|
}
|
|
|
|
TEST(MsgConversion, odomInfoToStatisticsNoFeatures)
|
|
{
|
|
rtabmap::OdometryInfo info;
|
|
info.features = 0;
|
|
info.reg.inliers = 10;
|
|
|
|
EXPECT_FLOAT_EQ(odomInfoToStatistics(info).at("Odometry/MatchesRatio/"), 0.0f)
|
|
<< "must not divide by zero";
|
|
}
|
|
|
|
/////////////////////////
|
|
// MapGraph / MapData
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, mapGraphRoundTrip)
|
|
{
|
|
std::map<int, rtabmap::Transform> poses;
|
|
poses.insert(std::make_pair(1, rtabmap::Transform(1, 0, 0, 0, 0, 0)));
|
|
poses.insert(std::make_pair(2, sampleTransform()));
|
|
|
|
std::multimap<int, rtabmap::Link> links;
|
|
links.insert(std::make_pair(1, rtabmap::Link(
|
|
1, 2, rtabmap::Link::kNeighbor, sampleTransform(),
|
|
cv::Mat::eye(6, 6, CV_64FC1) * 2.0)));
|
|
links.insert(std::make_pair(2, rtabmap::Link(
|
|
2, 1, rtabmap::Link::kGlobalClosure, rtabmap::Transform::getIdentity(),
|
|
cv::Mat::eye(6, 6, CV_64FC1))));
|
|
|
|
const rtabmap::Transform mapToOdom(0.5f, -0.5f, 0.0f, 0.0f, 0.0f, 0.1f);
|
|
|
|
rtabmap_msgs::msg::MapGraph msg;
|
|
mapGraphToROS(poses, links, mapToOdom, msg);
|
|
ASSERT_EQ(msg.poses.size(), poses.size());
|
|
ASSERT_EQ(msg.poses_id.size(), poses.size());
|
|
ASSERT_EQ(msg.links.size(), links.size());
|
|
|
|
std::map<int, rtabmap::Transform> outPoses;
|
|
std::multimap<int, rtabmap::Link> outLinks;
|
|
rtabmap::Transform outMapToOdom;
|
|
mapGraphFromROS(msg, outPoses, outLinks, outMapToOdom);
|
|
|
|
ASSERT_EQ(outPoses.size(), poses.size());
|
|
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
|
{
|
|
ASSERT_TRUE(outPoses.find(iter->first) != outPoses.end()) << "missing pose " << iter->first;
|
|
expectTransformNear(outPoses.at(iter->first), iter->second);
|
|
}
|
|
|
|
ASSERT_EQ(outLinks.size(), links.size());
|
|
for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
|
{
|
|
std::multimap<int, rtabmap::Link>::const_iterator found = outLinks.find(iter->first);
|
|
ASSERT_TRUE(found != outLinks.end()) << "missing link from " << iter->first;
|
|
EXPECT_EQ(found->second.from(), iter->second.from());
|
|
EXPECT_EQ(found->second.to(), iter->second.to());
|
|
EXPECT_EQ(found->second.type(), iter->second.type());
|
|
}
|
|
|
|
expectTransformNear(outMapToOdom, mapToOdom);
|
|
}
|
|
|
|
TEST(MsgConversion, mapGraphEmptyRoundTrip)
|
|
{
|
|
rtabmap_msgs::msg::MapGraph msg;
|
|
mapGraphToROS(std::map<int, rtabmap::Transform>(), std::multimap<int, rtabmap::Link>(),
|
|
rtabmap::Transform(), msg);
|
|
|
|
EXPECT_TRUE(msg.poses.empty());
|
|
EXPECT_TRUE(msg.links.empty());
|
|
|
|
std::map<int, rtabmap::Transform> poses;
|
|
std::multimap<int, rtabmap::Link> links;
|
|
rtabmap::Transform mapToOdom;
|
|
mapGraphFromROS(msg, poses, links, mapToOdom);
|
|
|
|
EXPECT_TRUE(poses.empty());
|
|
EXPECT_TRUE(links.empty());
|
|
EXPECT_TRUE(mapToOdom.isNull()) << "a null map_to_odom must survive as null";
|
|
}
|
|
|
|
TEST(MsgConversion, mapDataRoundTrip)
|
|
{
|
|
std::map<int, rtabmap::Transform> poses;
|
|
poses.insert(std::make_pair(1, sampleTransform()));
|
|
|
|
std::multimap<int, rtabmap::Link> links;
|
|
links.insert(std::make_pair(1, rtabmap::Link(
|
|
1, 2, rtabmap::Link::kNeighbor, sampleTransform())));
|
|
|
|
std::map<int, rtabmap::Signature> signatures;
|
|
rtabmap::Signature sig(1, 0, 3, 1234.5, "my_label", sampleTransform());
|
|
signatures.insert(std::make_pair(1, sig));
|
|
|
|
const rtabmap::Transform mapToOdom = rtabmap::Transform::getIdentity();
|
|
|
|
rtabmap_msgs::msg::MapData msg;
|
|
mapDataToROS(poses, links, signatures, mapToOdom, msg);
|
|
ASSERT_EQ(msg.nodes.size(), signatures.size());
|
|
ASSERT_EQ(msg.graph.poses.size(), poses.size());
|
|
|
|
std::map<int, rtabmap::Transform> outPoses;
|
|
std::multimap<int, rtabmap::Link> outLinks;
|
|
std::map<int, rtabmap::Signature> outSignatures;
|
|
rtabmap::Transform outMapToOdom;
|
|
mapDataFromROS(msg, outPoses, outLinks, outSignatures, outMapToOdom);
|
|
|
|
EXPECT_EQ(outPoses.size(), poses.size());
|
|
EXPECT_EQ(outLinks.size(), links.size());
|
|
ASSERT_EQ(outSignatures.size(), signatures.size());
|
|
ASSERT_TRUE(outSignatures.find(1) != outSignatures.end());
|
|
EXPECT_EQ(outSignatures.at(1).id(), sig.id());
|
|
EXPECT_EQ(outSignatures.at(1).getLabel(), sig.getLabel());
|
|
EXPECT_EQ(outSignatures.at(1).getWeight(), sig.getWeight());
|
|
EXPECT_NEAR(outSignatures.at(1).getStamp(), sig.getStamp(), 1e-6);
|
|
}
|
|
|
|
/////////////////////////
|
|
// Node / Signature
|
|
/////////////////////////
|
|
|
|
namespace {
|
|
|
|
rtabmap::Signature sampleSignature()
|
|
{
|
|
rtabmap::Signature s(7, 2, 3, 1234.5, "node_label", sampleTransform());
|
|
|
|
std::multimap<int, int> words;
|
|
std::vector<cv::KeyPoint> kpts;
|
|
std::vector<cv::Point3f> pts3;
|
|
cv::Mat descriptors(2, 4, CV_32FC1);
|
|
for(int i=0; i<2; ++i)
|
|
{
|
|
words.insert(std::make_pair(100 + i, i));
|
|
kpts.push_back(cv::KeyPoint(cv::Point2f(10.0f * i, 20.0f * i), 7.0f));
|
|
pts3.push_back(cv::Point3f(1.0f * i, 2.0f * i, 3.0f * i));
|
|
for(int j=0; j<4; ++j)
|
|
{
|
|
descriptors.at<float>(i, j) = float(i * 4 + j);
|
|
}
|
|
}
|
|
s.setWords(words, kpts, pts3, descriptors);
|
|
return s;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, nodeRoundTrip)
|
|
{
|
|
const rtabmap::Signature in = sampleSignature();
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeToROS(in, msg);
|
|
const rtabmap::Signature out = nodeFromROS(msg);
|
|
|
|
EXPECT_EQ(out.id(), in.id());
|
|
EXPECT_EQ(out.mapId(), in.mapId());
|
|
EXPECT_EQ(out.getWeight(), in.getWeight());
|
|
EXPECT_NEAR(out.getStamp(), in.getStamp(), 1e-6);
|
|
EXPECT_EQ(out.getLabel(), in.getLabel());
|
|
expectTransformNear(out.getPose(), in.getPose());
|
|
|
|
// Visual words: ids, keypoints, 3D points and descriptors.
|
|
ASSERT_EQ(out.getWords().size(), in.getWords().size());
|
|
EXPECT_TRUE(std::equal(out.getWords().begin(), out.getWords().end(), in.getWords().begin()));
|
|
|
|
ASSERT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size());
|
|
for(size_t i=0; i<in.getWordsKpts().size(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out.getWordsKpts()[i].pt.x, in.getWordsKpts()[i].pt.x) << "kpt " << i;
|
|
EXPECT_FLOAT_EQ(out.getWordsKpts()[i].pt.y, in.getWordsKpts()[i].pt.y) << "kpt " << i;
|
|
}
|
|
|
|
ASSERT_EQ(out.getWords3().size(), in.getWords3().size());
|
|
for(size_t i=0; i<in.getWords3().size(); ++i)
|
|
{
|
|
EXPECT_FLOAT_EQ(out.getWords3()[i].x, in.getWords3()[i].x) << "pt3 " << i;
|
|
EXPECT_FLOAT_EQ(out.getWords3()[i].z, in.getWords3()[i].z) << "pt3 " << i;
|
|
}
|
|
|
|
ASSERT_EQ(out.getWordsDescriptors().rows, in.getWordsDescriptors().rows);
|
|
ASSERT_EQ(out.getWordsDescriptors().cols, in.getWordsDescriptors().cols);
|
|
EXPECT_EQ(cv::countNonZero(out.getWordsDescriptors() != in.getWordsDescriptors()), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, nodeGroundTruthRoundTrip)
|
|
{
|
|
// The ground truth travels in the Node's SensorData sub-message but is written and
|
|
// read by the Node conversion itself.
|
|
rtabmap::Signature in(7, 2, 3, 1234.5, "node_label",
|
|
sampleTransform(), rtabmap::Transform(9.0f, 8.0f, 7.0f, 0.0f, 0.0f, 0.0f));
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeToROS(in, msg);
|
|
const rtabmap::Signature out = nodeFromROS(msg);
|
|
|
|
expectTransformNear(out.getGroundTruthPose(), in.getGroundTruthPose());
|
|
}
|
|
|
|
TEST(MsgConversion, nodeInfoRoundTripCarriesNoSensorData)
|
|
{
|
|
const rtabmap::Signature in = sampleSignature();
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeInfoToROS(in, msg);
|
|
const rtabmap::Signature out = nodeInfoFromROS(msg);
|
|
|
|
EXPECT_EQ(out.id(), in.id());
|
|
EXPECT_EQ(out.mapId(), in.mapId());
|
|
EXPECT_EQ(out.getWeight(), in.getWeight());
|
|
EXPECT_EQ(out.getLabel(), in.getLabel());
|
|
expectTransformNear(out.getPose(), in.getPose());
|
|
}
|
|
|
|
TEST(MsgConversion, nodeDataRoundTripCarriesWords)
|
|
{
|
|
const rtabmap::Signature in = sampleSignature();
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeDataToROS(in, msg);
|
|
const rtabmap::Signature out = nodeDataFromROS(msg);
|
|
|
|
EXPECT_EQ(out.getWords().size(), in.getWords().size());
|
|
EXPECT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size());
|
|
EXPECT_EQ(out.getWords3().size(), in.getWords3().size());
|
|
}
|
|
|
|
TEST(MsgConversion, nodeEmptyRoundTrip)
|
|
{
|
|
rtabmap::Signature in(1);
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeToROS(in, msg);
|
|
const rtabmap::Signature out = nodeFromROS(msg);
|
|
|
|
EXPECT_EQ(out.id(), 1);
|
|
EXPECT_TRUE(out.getWords().empty());
|
|
EXPECT_TRUE(out.getWordsKpts().empty());
|
|
EXPECT_TRUE(out.getWordsDescriptors().empty());
|
|
}
|
|
|
|
/////////////////////////
|
|
// SensorData
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, sensorDataRoundTrip)
|
|
{
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
|
|
const rtabmap::CameraModel model(
|
|
"cam", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(),
|
|
rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f));
|
|
|
|
rtabmap::SensorData in(cv::Mat(), cv::Mat(), model, 42, 1234.5);
|
|
in.setGroundTruth(sampleTransform());
|
|
in.setGPS(rtabmap::GPS(1234.5, -71.9, 45.4, 100.0, 5.0, 90.0));
|
|
|
|
rtabmap_msgs::msg::SensorData msg;
|
|
sensorDataToROS(in, msg, "base_link");
|
|
EXPECT_EQ(msg.header.frame_id, "base_link");
|
|
|
|
const rtabmap::SensorData out = sensorDataFromROS(msg);
|
|
|
|
EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
|
|
|
|
// sensorDataToROS writes ground_truth_pose into the message, but sensorDataFromROS
|
|
// deliberately does not read it back: the ground truth is owned by the enclosing
|
|
// Node conversion (nodeFromROS feeds it to the Signature constructor). See
|
|
// nodeGroundTruthRoundTrip for the round trip that does preserve it.
|
|
EXPECT_FALSE(transformFromPoseMsg(msg.ground_truth_pose).isNull())
|
|
<< "the message must still carry the ground truth for nodeFromROS";
|
|
EXPECT_TRUE(out.groundTruth().isNull())
|
|
<< "sensorDataFromROS does not restore the ground truth";
|
|
|
|
ASSERT_EQ(out.cameraModels().size(), 1u);
|
|
EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9);
|
|
EXPECT_NEAR(out.cameraModels()[0].cx(), 320.0, 1e-9);
|
|
expectTransformNear(
|
|
out.cameraModels()[0].localTransform(), model.localTransform());
|
|
|
|
EXPECT_NEAR(out.gps().longitude(), in.gps().longitude(), 1e-9);
|
|
EXPECT_NEAR(out.gps().latitude(), in.gps().latitude(), 1e-9);
|
|
EXPECT_NEAR(out.gps().altitude(), in.gps().altitude(), 1e-9);
|
|
EXPECT_NEAR(out.gps().bearing(), in.gps().bearing(), 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, sensorDataUserDataRoundTrip)
|
|
{
|
|
rtabmap::SensorData in;
|
|
in.setStamp(10.0);
|
|
in.setUserData((cv::Mat_<int>(1, 3) << 7, 8, 9));
|
|
|
|
rtabmap_msgs::msg::SensorData msg;
|
|
sensorDataToROS(in, msg);
|
|
const rtabmap::SensorData out = sensorDataFromROS(msg);
|
|
|
|
const cv::Mat data = out.userDataRaw().empty()
|
|
? rtabmap::uncompressData(out.userDataCompressed())
|
|
: out.userDataRaw();
|
|
ASSERT_FALSE(data.empty());
|
|
ASSERT_EQ(data.cols, 3);
|
|
EXPECT_EQ(data.at<int>(0, 0), 7);
|
|
EXPECT_EQ(data.at<int>(0, 2), 9);
|
|
}
|
|
|
|
/////////////////////////
|
|
// Statistics / Info
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, infoRoundTrip)
|
|
{
|
|
rtabmap::Statistics in;
|
|
in.setExtended(true);
|
|
in.setRefImageId(5);
|
|
in.setLoopClosureId(9);
|
|
in.setProximityDetectionId(11);
|
|
in.setStamp(1234.5);
|
|
in.setLoopClosureTransform(sampleTransform());
|
|
in.setWmState(std::vector<int>{1, 2, 3});
|
|
|
|
std::map<int, float> posterior;
|
|
posterior.insert(std::make_pair(1, 0.25f));
|
|
posterior.insert(std::make_pair(2, 0.75f));
|
|
in.setPosterior(posterior);
|
|
|
|
std::map<int, int> weights;
|
|
weights.insert(std::make_pair(1, 10));
|
|
in.setWeights(weights);
|
|
|
|
std::map<int, std::string> labels;
|
|
labels.insert(std::make_pair(1, "kitchen"));
|
|
in.setLabels(labels);
|
|
|
|
in.addStatistic("Some/Stat/", 3.5f);
|
|
|
|
rtabmap_msgs::msg::Info msg;
|
|
infoToROS(in, msg);
|
|
|
|
// An unstamped header is filled from the statistics, so infoFromROS recovers the
|
|
// stamp without the caller doing anything. Only to double precision, though.
|
|
EXPECT_NEAR(timestampFromROS(msg.header.stamp), in.stamp(), 1e-6);
|
|
EXPECT_TRUE(msg.header.frame_id.empty()) << "the frame id is always the caller's job";
|
|
|
|
rtabmap::Statistics out;
|
|
infoFromROS(msg, out);
|
|
|
|
EXPECT_EQ(out.refImageId(), in.refImageId());
|
|
EXPECT_EQ(out.loopClosureId(), in.loopClosureId());
|
|
EXPECT_EQ(out.proximityDetectionId(), in.proximityDetectionId());
|
|
EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
|
|
expectTransformNear(out.loopClosureTransform(), in.loopClosureTransform());
|
|
EXPECT_EQ(out.wmState(), in.wmState());
|
|
|
|
ASSERT_EQ(out.posterior().size(), in.posterior().size());
|
|
EXPECT_FLOAT_EQ(out.posterior().at(1), 0.25f);
|
|
EXPECT_FLOAT_EQ(out.posterior().at(2), 0.75f);
|
|
|
|
ASSERT_EQ(out.weights().size(), in.weights().size());
|
|
EXPECT_EQ(out.weights().at(1), 10);
|
|
|
|
ASSERT_EQ(out.labels().size(), in.labels().size());
|
|
EXPECT_EQ(out.labels().at(1), "kitchen");
|
|
|
|
ASSERT_TRUE(out.data().find("Some/Stat/") != out.data().end());
|
|
EXPECT_FLOAT_EQ(out.data().at("Some/Stat/"), 3.5f);
|
|
}
|
|
|
|
/////////////////////////
|
|
// PointCloud2 helpers
|
|
/////////////////////////
|
|
|
|
namespace {
|
|
|
|
/// Builds a dense, unorganized XYZ float cloud from the given points.
|
|
sensor_msgs::msg::PointCloud2 makeXYZCloud(const std::vector<cv::Point3f> & points)
|
|
{
|
|
sensor_msgs::msg::PointCloud2 cloud;
|
|
cloud.height = 1;
|
|
cloud.width = points.size();
|
|
cloud.is_bigendian = false;
|
|
cloud.is_dense = true;
|
|
cloud.fields.resize(3);
|
|
const char * names[3] = {"x", "y", "z"};
|
|
for(int i=0; i<3; ++i)
|
|
{
|
|
cloud.fields[i].name = names[i];
|
|
cloud.fields[i].offset = 4 * i;
|
|
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
|
|
cloud.fields[i].count = 1;
|
|
}
|
|
cloud.point_step = 12;
|
|
cloud.row_step = cloud.point_step * cloud.width;
|
|
cloud.data.resize(cloud.row_step * cloud.height);
|
|
for(size_t i=0; i<points.size(); ++i)
|
|
{
|
|
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
|
|
p[0] = points[i].x;
|
|
p[1] = points[i].y;
|
|
p[2] = points[i].z;
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
|
|
{
|
|
const float * p = reinterpret_cast<const float *>(&cloud.data[index * cloud.point_step]);
|
|
return cv::Point3f(p[0], p[1], p[2]);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, transformPointCloudTranslation)
|
|
{
|
|
const std::vector<cv::Point3f> points = {{1.0f, 2.0f, 3.0f}, {-1.0f, 0.0f, 1.0f}};
|
|
const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points);
|
|
|
|
Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
|
|
t(0, 3) = 10.0f;
|
|
t(1, 3) = 20.0f;
|
|
t(2, 3) = 30.0f;
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
transformPointCloud(t, in, out);
|
|
|
|
ASSERT_EQ(out.width, in.width);
|
|
ASSERT_EQ(out.point_step, in.point_step);
|
|
for(size_t i=0; i<points.size(); ++i)
|
|
{
|
|
const cv::Point3f p = readXYZ(out, i);
|
|
EXPECT_NEAR(p.x, points[i].x + 10.0f, 1e-4) << "point " << i;
|
|
EXPECT_NEAR(p.y, points[i].y + 20.0f, 1e-4) << "point " << i;
|
|
EXPECT_NEAR(p.z, points[i].z + 30.0f, 1e-4) << "point " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, transformPointCloudRotation)
|
|
{
|
|
// 90 degrees about z maps (1,0,0) to (0,1,0).
|
|
const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
|
|
const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points);
|
|
|
|
const Eigen::Matrix4f t =
|
|
rtabmap::Transform(0, 0, 0, 0, 0, M_PI/2.0).toEigen4f();
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
transformPointCloud(t, in, out);
|
|
|
|
const cv::Point3f p = readXYZ(out, 0);
|
|
EXPECT_NEAR(p.x, 0.0f, 1e-5);
|
|
EXPECT_NEAR(p.y, 1.0f, 1e-5);
|
|
EXPECT_NEAR(p.z, 0.0f, 1e-5);
|
|
}
|
|
|
|
TEST(MsgConversion, transformPointCloudIdentityPreservesMetadata)
|
|
{
|
|
const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 2.0f, 3.0f}});
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
transformPointCloud(Eigen::Matrix4f::Identity(), in, out);
|
|
|
|
EXPECT_EQ(out.height, in.height);
|
|
EXPECT_EQ(out.width, in.width);
|
|
EXPECT_EQ(out.point_step, in.point_step);
|
|
EXPECT_EQ(out.row_step, in.row_step);
|
|
EXPECT_EQ(out.is_dense, in.is_dense);
|
|
ASSERT_EQ(out.fields.size(), in.fields.size());
|
|
for(size_t i=0; i<in.fields.size(); ++i)
|
|
{
|
|
EXPECT_EQ(out.fields[i].name, in.fields[i].name) << "field " << i;
|
|
}
|
|
|
|
const cv::Point3f p = readXYZ(out, 0);
|
|
EXPECT_NEAR(p.x, 1.0f, 1e-5);
|
|
EXPECT_NEAR(p.y, 2.0f, 1e-5);
|
|
EXPECT_NEAR(p.z, 3.0f, 1e-5);
|
|
}
|
|
|
|
namespace {
|
|
|
|
// A robot driving straight at a wall while the lidar sweeps.
|
|
// A 10 Hz lidar: 100 samples 1 ms apart, so the sweep spans 99 ms and scans repeat
|
|
// every 100 ms. Scan N covers [1000.000, 1000.099], scan N+1 covers [1000.100, 1000.199].
|
|
constexpr size_t kScanPoints = 100; // samples per sweep
|
|
constexpr double kScanStep = 0.001; // s between consecutive samples
|
|
constexpr double kScanSpan = kScanStep * (kScanPoints - 1); // 0.099 s, first -> last
|
|
constexpr float kWallDistance = 5.0f; // m, distance to the wall at the first point
|
|
constexpr float kSpeed = 1.0f; // m/s forward (+x)
|
|
|
|
/// How the per-point time channel is encoded. deskew() accepts three datatypes, and
|
|
/// FLOAT64 differs from the other two: it carries ABSOLUTE stamps (with an automatic
|
|
/// ms/us/ns unit guess), while UINT32 and FLOAT32 carry offsets from the header stamp.
|
|
enum TimeEncoding
|
|
{
|
|
kOffsetSecFloat32, ///< FLOAT32 seconds, relative to header.stamp
|
|
kOffsetNsecUint32, ///< UINT32 nanoseconds, relative to header.stamp
|
|
kAbsoluteSecFloat64, ///< FLOAT64 absolute seconds
|
|
kAbsoluteMsecFloat64 ///< FLOAT64 absolute milliseconds (auto-scaled by deskew)
|
|
};
|
|
|
|
/// Organized-cloud layout. deskew() picks its traversal from width>height, so the two
|
|
/// orderings exercise different loops.
|
|
enum ScanLayout
|
|
{
|
|
kTimeOnColumns, ///< Ouster style: width=time samples, height=rings
|
|
kTimeOnRows ///< Velodyne style: height=time samples, width=rings
|
|
};
|
|
|
|
/**
|
|
* Builds the raw (skewed) scan of a flat wall captured while moving forward.
|
|
*
|
|
* Each time sample is taken 1 ms after the previous one, by which time the robot has
|
|
* closed in on the wall by kSpeed * elapsed. Expressed in the sensor frame at capture
|
|
* time, the wall therefore appears to slide towards the robot: a straight wall is
|
|
* recorded as a slanted line. Deskewing must undo exactly that.
|
|
*
|
|
* @param headerStamp absolute stamp put in the message header
|
|
* @param firstPointOffset time of the first sample relative to the header stamp
|
|
* @param encoding how to write the time channel
|
|
* @param layout whether time runs along columns or rows
|
|
* @param rings number of rings (the non-time dimension)
|
|
* @param fieldName name of the time channel
|
|
* @param descendingTime emit the samples newest-first, which deskew has to detect
|
|
* @param displacement distance travelled as a function of time since the first
|
|
* sample; defaults to the constant-velocity kSpeed * elapsed
|
|
*/
|
|
sensor_msgs::msg::PointCloud2 makeSkewedWallScan(
|
|
double headerStamp,
|
|
double firstPointOffset,
|
|
TimeEncoding encoding = kOffsetSecFloat32,
|
|
ScanLayout layout = kTimeOnColumns,
|
|
size_t rings = 1,
|
|
const std::string & fieldName = "t",
|
|
bool descendingTime = false,
|
|
const std::function<double(double)> & displacement = nullptr)
|
|
{
|
|
const bool timeIs64Bit =
|
|
encoding == kAbsoluteSecFloat64 || encoding == kAbsoluteMsecFloat64;
|
|
// Keep the 8-byte time channel aligned: x,y,z then 4 bytes of padding.
|
|
const uint32_t timeOffset = timeIs64Bit ? 16 : 12;
|
|
const uint32_t pointStep = timeIs64Bit ? 24 : 16;
|
|
|
|
sensor_msgs::msg::PointCloud2 cloud;
|
|
cloud.header.stamp = timestampToROS(headerStamp);
|
|
cloud.header.frame_id = "base_link";
|
|
cloud.is_bigendian = false;
|
|
cloud.is_dense = true;
|
|
if(layout == kTimeOnColumns)
|
|
{
|
|
cloud.width = kScanPoints;
|
|
cloud.height = rings;
|
|
}
|
|
else
|
|
{
|
|
cloud.width = rings;
|
|
cloud.height = kScanPoints;
|
|
}
|
|
|
|
cloud.fields.resize(4);
|
|
const char * xyz[3] = {"x", "y", "z"};
|
|
for(int i=0; i<3; ++i)
|
|
{
|
|
cloud.fields[i].name = xyz[i];
|
|
cloud.fields[i].offset = 4 * i;
|
|
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
|
|
cloud.fields[i].count = 1;
|
|
}
|
|
cloud.fields[3].name = fieldName;
|
|
cloud.fields[3].offset = timeOffset;
|
|
cloud.fields[3].datatype =
|
|
encoding == kOffsetNsecUint32 ? sensor_msgs::msg::PointField::UINT32 :
|
|
timeIs64Bit ? sensor_msgs::msg::PointField::FLOAT64 :
|
|
sensor_msgs::msg::PointField::FLOAT32;
|
|
cloud.fields[3].count = 1;
|
|
|
|
cloud.point_step = pointStep;
|
|
cloud.row_step = cloud.point_step * cloud.width;
|
|
cloud.data.resize(cloud.row_step * cloud.height);
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i) // position in the message
|
|
{
|
|
// When descending, the point stored first is the one captured last.
|
|
const size_t sample = descendingTime ? (kScanPoints - 1 - i) : i;
|
|
const double elapsed = double(sample) * kScanStep; // since the first capture
|
|
const double offset = firstPointOffset + elapsed; // relative to the header
|
|
const double absolute = headerStamp + offset;
|
|
|
|
for(size_t r=0; r<rings; ++r)
|
|
{
|
|
const size_t row = (layout == kTimeOnColumns) ? r : i;
|
|
const size_t col = (layout == kTimeOnColumns) ? i : r;
|
|
unsigned char * base = &cloud.data[row * cloud.row_step + col * cloud.point_step];
|
|
|
|
float * p = reinterpret_cast<float *>(base);
|
|
// The robot has closed in on the wall by this much when the sample was taken.
|
|
const double travelled = displacement ? displacement(elapsed) : kSpeed * elapsed;
|
|
p[0] = kWallDistance - float(travelled); // the skew
|
|
p[1] = -1.0f + 2.0f * float(sample) / float(kScanPoints - 1);
|
|
p[2] = 0.1f * float(r); // one plane per ring
|
|
|
|
switch(encoding)
|
|
{
|
|
case kOffsetSecFloat32:
|
|
*reinterpret_cast<float *>(base + timeOffset) = float(offset);
|
|
break;
|
|
case kOffsetNsecUint32:
|
|
*reinterpret_cast<uint32_t *>(base + timeOffset) =
|
|
uint32_t(std::llround(offset * 1e9));
|
|
break;
|
|
case kAbsoluteSecFloat64:
|
|
*reinterpret_cast<double *>(base + timeOffset) = absolute;
|
|
break;
|
|
case kAbsoluteMsecFloat64:
|
|
*reinterpret_cast<double *>(base + timeOffset) = absolute * 1e3;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return cloud;
|
|
}
|
|
|
|
/// Reads x of the point at (time sample, ring) for the given layout.
|
|
float readWallX(const sensor_msgs::msg::PointCloud2 & cloud, size_t sample, size_t ring,
|
|
ScanLayout layout)
|
|
{
|
|
const size_t row = (layout == kTimeOnColumns) ? ring : sample;
|
|
const size_t col = (layout == kTimeOnColumns) ? sample : ring;
|
|
return *reinterpret_cast<const float *>(
|
|
&cloud.data[row * cloud.row_step + col * cloud.point_step]);
|
|
}
|
|
|
|
float readField(const sensor_msgs::msg::PointCloud2 & cloud, size_t index, size_t field)
|
|
{
|
|
return *reinterpret_cast<const float *>(
|
|
&cloud.data[index * cloud.point_step + cloud.fields[field].offset]);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, deskewConstantVelocityHeaderAtFirstPoint)
|
|
{
|
|
const double firstPointStamp = 1000.0;
|
|
|
|
// Header stamped at the first point, so "t" runs 0 .. +0.100 s.
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0);
|
|
ASSERT_NEAR(readField(in, 0, 0), kWallDistance, 1e-4) << "first point is unskewed";
|
|
ASSERT_NEAR(readField(in, kScanPoints-1, 0), kWallDistance - float(kScanSpan), 1e-4)
|
|
<< "last point is skewed by v*0.099s = 9.9 cm";
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
// Everything collapses back onto the wall at its original distance.
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readField(out, i, 0), kWallDistance, 1e-4) << "x of point " << i;
|
|
EXPECT_NEAR(readField(out, i, 1), readField(in, i, 1), 1e-6) << "y of point " << i;
|
|
EXPECT_NEAR(readField(out, i, 2), 0.0f, 1e-6) << "z of point " << i;
|
|
EXPECT_FLOAT_EQ(readField(out, i, 3), 0.0f)
|
|
<< "t must be zeroed to mark the cloud as deskewed, point " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewConstantVelocityHeaderAtLastPoint)
|
|
{
|
|
const double firstPointStamp = 1000.0;
|
|
const double lastPointStamp = firstPointStamp + kScanSpan;
|
|
|
|
// Same physical scan, but stamped at the last point: "t" runs -0.100 .. 0 s.
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(lastPointStamp, -kScanSpan);
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
// Still a straight line, but now expressed in the frame at the END of the scan,
|
|
// by which point the robot has advanced kSpeed * kScanSpan = 9.9 cm.
|
|
const float expected = kWallDistance - kSpeed * float(kScanSpan);
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readField(out, i, 0), expected, 1e-4) << "x of point " << i;
|
|
}
|
|
|
|
// The line is flat to well under a millimeter: that is the deskewing working,
|
|
// independently of which end of the scan the frame is anchored to.
|
|
float minX = readField(out, 0, 0);
|
|
float maxX = minX;
|
|
for(size_t i=1; i<kScanPoints; ++i)
|
|
{
|
|
minX = std::min(minX, readField(out, i, 0));
|
|
maxX = std::max(maxX, readField(out, i, 0));
|
|
}
|
|
EXPECT_LT(maxX - minX, 1e-3f) << "deskewed scan must be flat";
|
|
}
|
|
|
|
TEST(MsgConversion, deskewConstantVelocityStationaryRobotIsNoOp)
|
|
{
|
|
// With no motion there is nothing to correct, so the skewed input must come back
|
|
// unchanged -- this pins that the correction is driven by the velocity and not by
|
|
// something incidental to the time field.
|
|
const double firstPointStamp = 1000.0;
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0);
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(0, 0, 0, 0, 0, 0)));
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readField(out, i, 0), readField(in, i, 0), 1e-6) << "point " << i;
|
|
}
|
|
}
|
|
|
|
/// Deskews a wall scan built with the given encoding/layout and asserts it comes out
|
|
/// flat at the expected distance.
|
|
void expectDeskewRecoversWall(
|
|
TimeEncoding encoding,
|
|
ScanLayout layout,
|
|
size_t rings,
|
|
double headerStamp,
|
|
const std::string & fieldName = "t")
|
|
{
|
|
SCOPED_TRACE("encoding=" + std::to_string(int(encoding)) +
|
|
" layout=" + std::to_string(int(layout)) +
|
|
" rings=" + std::to_string(rings) +
|
|
" field=" + fieldName);
|
|
|
|
const sensor_msgs::msg::PointCloud2 in =
|
|
makeSkewedWallScan(headerStamp, 0.0, encoding, layout, rings, fieldName);
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
for(size_t r=0; r<rings; ++r)
|
|
{
|
|
EXPECT_NEAR(readWallX(out, i, r, layout), kWallDistance, 1e-3)
|
|
<< "sample " << i << " ring " << r;
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeAsFloat32SecondsOffset)
|
|
{
|
|
expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 1, 1000.0);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeAsUint32NanosecondsOffset)
|
|
{
|
|
// UINT32 offsets are unsigned, so the header can only sit at or before the scan.
|
|
expectDeskewRecoversWall(kOffsetNsecUint32, kTimeOnColumns, 1, 1000.0);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeAsFloat64AbsoluteSeconds)
|
|
{
|
|
// FLOAT64 carries absolute stamps rather than offsets.
|
|
expectDeskewRecoversWall(kAbsoluteSecFloat64, kTimeOnColumns, 1, 1000.0);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeAsFloat64AbsoluteMilliseconds)
|
|
{
|
|
// Above 1e12 deskew treats FLOAT64 stamps as milliseconds and rescales them, so
|
|
// this needs a realistic epoch: 1.7e9 s is 1.7e12 ms.
|
|
expectDeskewRecoversWall(kAbsoluteMsecFloat64, kTimeOnColumns, 1, 1.7e9);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeOnColumnsWithMultipleRings)
|
|
{
|
|
// Ouster layout: width=101 samples > height=4 rings.
|
|
expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 4, 1000.0);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewTimeOnRowsWithMultipleRings)
|
|
{
|
|
// Velodyne layout: height=101 samples > width=4 rings, which takes the other loop.
|
|
expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnRows, 4, 1000.0);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewLayoutsAgree)
|
|
{
|
|
// The same scan expressed in either layout must deskew to the same geometry.
|
|
const double headerStamp = 1000.0;
|
|
const size_t rings = 4;
|
|
|
|
sensor_msgs::msg::PointCloud2 byColumns, byRows;
|
|
ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, rings),
|
|
byColumns, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnRows, rings),
|
|
byRows, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
for(size_t r=0; r<rings; ++r)
|
|
{
|
|
EXPECT_NEAR(readWallX(byColumns, i, r, kTimeOnColumns),
|
|
readWallX(byRows, i, r, kTimeOnRows), 1e-6)
|
|
<< "sample " << i << " ring " << r;
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewAcceptsEveryTimeFieldName)
|
|
{
|
|
for(const std::string & name : {"t", "time", "stamps", "timestamp"})
|
|
{
|
|
expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 1, 1000.0, name);
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewHandlesDescendingTimestamps)
|
|
{
|
|
// Some drivers emit the sweep newest-first. deskew detects that the channel is not
|
|
// ascending and rescans it for the true min/max before interpolating.
|
|
const double headerStamp = 1000.0;
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
|
|
headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t",
|
|
/*descendingTime=*/true);
|
|
|
|
// Sanity: the stored order really is newest-first.
|
|
ASSERT_NEAR(readWallX(in, 0, 0, kTimeOnColumns), kWallDistance - float(kScanSpan), 1e-4);
|
|
ASSERT_NEAR(readWallX(in, kScanPoints-1, 0, kTimeOnColumns), kWallDistance, 1e-4);
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
|
|
<< "sample " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewRejectsUnknownTimeFieldName)
|
|
{
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
|
|
1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "elapsed");
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
EXPECT_FALSE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
}
|
|
|
|
TEST(MsgConversion, deskewRejectsUnsupportedTimeDatatype)
|
|
{
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
|
|
in.fields[3].datatype = sensor_msgs::msg::PointField::INT32; // 4 bytes, but not 6/7/8
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
EXPECT_FALSE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
}
|
|
|
|
TEST(MsgConversion, deskewWithRotationIsAnchoredAtTheHeaderStamp)
|
|
{
|
|
// A pure yaw rate makes the correction a pure rotation about z, so it can be checked
|
|
// exactly: each sample must be rotated by yawRate * (its time - the header stamp),
|
|
// with its distance from the origin unchanged. This is what pins the correction to
|
|
// the header stamp -- there is no other reference time involved.
|
|
const double headerStamp = 1000.0;
|
|
const double yawRate = 0.5; // rad/s
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0);
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(0, 0, 0, 0, 0, yawRate)));
|
|
|
|
auto xy = [](const sensor_msgs::msg::PointCloud2 & c, size_t i) {
|
|
const float * p = reinterpret_cast<const float *>(&c.data[i * c.point_step]);
|
|
return std::make_pair(p[0], p[1]);
|
|
};
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
const std::pair<float, float> a = xy(in, i);
|
|
const std::pair<float, float> b = xy(out, i);
|
|
const double dt = double(i) * kScanStep; // sample 0 sits at the header stamp
|
|
|
|
EXPECT_NEAR(std::hypot(b.first, b.second), std::hypot(a.first, a.second), 1e-4)
|
|
<< "a rotation must preserve the range of sample " << i;
|
|
EXPECT_NEAR(std::atan2(b.second, b.first) - std::atan2(a.second, a.first),
|
|
yawRate * dt, 1e-4)
|
|
<< "sample " << i << " must be rotated by yawRate*dt";
|
|
}
|
|
|
|
// Spelling out the i=0 case: dt is zero there, so that sample is untouched.
|
|
EXPECT_FLOAT_EQ(xy(out, 0).first, xy(in, 0).first);
|
|
EXPECT_FLOAT_EQ(xy(out, 0).second, xy(in, 0).second);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewPassesThroughWhenThereIsNoTimeSpread)
|
|
{
|
|
// A driver that leaves the time channel at zero gives a scan with no time spread.
|
|
// There is nothing to correct, so the cloud must come back unchanged rather than
|
|
// being reported as a failure -- callers abort the frame on false.
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
*reinterpret_cast<float *>(&in.data[i * in.point_step + in.fields[3].offset]) = 0.0f;
|
|
}
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
EXPECT_EQ(out.data, in.data) << "the cloud must be returned untouched";
|
|
}
|
|
|
|
TEST(MsgConversion, deskewIsIdempotent)
|
|
{
|
|
// Deskewing zeroes the time channel to mark the cloud as done, so running deskew a
|
|
// second time (e.g. lidar_deskewing feeding icp_odometry) must be a silent no-op.
|
|
const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
|
|
const rtabmap::Transform velocity(kSpeed, 0, 0, 0, 0, 0);
|
|
|
|
sensor_msgs::msg::PointCloud2 once;
|
|
ASSERT_TRUE(deskew(in, once, velocity));
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
ASSERT_FLOAT_EQ(*reinterpret_cast<const float *>(
|
|
&once.data[i * once.point_step + once.fields[3].offset]), 0.0f)
|
|
<< "deskewing must zero the time channel, sample " << i;
|
|
}
|
|
|
|
sensor_msgs::msg::PointCloud2 twice;
|
|
ASSERT_TRUE(deskew(once, twice, velocity)) << "a second pass must not fail";
|
|
EXPECT_EQ(twice.data, once.data) << "a second pass must change nothing";
|
|
}
|
|
|
|
TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep)
|
|
{
|
|
// The ordering check only inspects the first and last samples, so a corrupt stamp in
|
|
// the middle is not detected. It must be clamped to the end of the sweep rather than
|
|
// extrapolated, which would fling the point far past the wall.
|
|
const double headerStamp = 1000.0;
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0);
|
|
const size_t corrupt = kScanPoints / 2;
|
|
*reinterpret_cast<float *>(
|
|
&in.data[corrupt * in.point_step + in.fields[3].offset]) = 0.5f; // 5x the sweep
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
|
|
|
|
// Clamped to the last sample's correction, so it lands within the sweep's own range
|
|
// rather than meters away. Every other sample is unaffected.
|
|
const float x = readWallX(out, corrupt, 0, kTimeOnColumns);
|
|
EXPECT_GE(x, kWallDistance - 1e-3f);
|
|
EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f)
|
|
<< "an unclamped ratio of ~5 would put this point ~0.45 m past the wall";
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
if(i == corrupt) continue;
|
|
EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
|
|
<< "uncorrupted sample " << i << " must be unaffected";
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewWithoutTimeFieldFails)
|
|
{
|
|
// Deskewing needs a per-point time field; a plain XYZ cloud cannot be deskewed.
|
|
const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
EXPECT_FALSE(deskew(in, out, rtabmap::Transform(1, 0, 0, 0, 0, 0)));
|
|
}
|
|
|
|
TEST(MsgConversion, deskewNullVelocityFails)
|
|
{
|
|
const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
EXPECT_FALSE(deskew(in, out, rtabmap::Transform()))
|
|
<< "a null velocity cannot deskew";
|
|
}
|
|
|
|
/////////////////////////
|
|
// RGBDImage
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, rgbdImageRoundTrip)
|
|
{
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
|
|
const rtabmap::CameraModel model(
|
|
"cam", cv::Size(4, 4), K, cv::Mat(), cv::Mat(), cv::Mat(),
|
|
rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f));
|
|
|
|
cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
|
|
rtabmap::SensorData in(rgb, depth, model, 1, 1234.5);
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
rgbdImageToROS(in, msg, "camera_link");
|
|
|
|
EXPECT_EQ(msg.rgb_camera_info.header.frame_id, "camera_link");
|
|
EXPECT_NEAR(timestampFromROS(msg.rgb_camera_info.header.stamp), 1234.5, 1e-6);
|
|
|
|
// The top-level header is stamped too, so rgbdImageFromROS recovers the stamp
|
|
// without the caller having to fill it in.
|
|
EXPECT_EQ(msg.header.frame_id, "camera_link");
|
|
EXPECT_NEAR(timestampFromROS(msg.header.stamp), 1234.5, 1e-6);
|
|
|
|
// The returned SensorData shallow-references the message buffers, so the message
|
|
// must outlive it -- see rgbdImageFromROSAliasesTheMessage.
|
|
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr held =
|
|
std::make_shared<const rtabmap_msgs::msg::RGBDImage>(msg);
|
|
const rtabmap::SensorData out = rgbdImageFromROS(held);
|
|
|
|
EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
|
|
ASSERT_EQ(out.cameraModels().size(), 1u);
|
|
EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9);
|
|
|
|
// The local transform is not carried by the message (CameraInfo has no such
|
|
// field); callers resolve it from TF, so it comes back as the default identity.
|
|
EXPECT_TRUE(out.cameraModels()[0].localTransform().isIdentity())
|
|
<< out.cameraModels()[0].localTransform().prettyPrint();
|
|
|
|
ASSERT_FALSE(out.imageRaw().empty());
|
|
EXPECT_EQ(out.imageRaw().type(), CV_8UC3);
|
|
EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0);
|
|
|
|
ASSERT_FALSE(out.depthRaw().empty());
|
|
EXPECT_EQ(out.depthRaw().type(), CV_16UC1);
|
|
EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, rgbdImageFromROSAliasesTheMessage)
|
|
{
|
|
// rgbdImageFromROS deliberately avoids copying the pixels: the SensorData it returns
|
|
// points into the message's own buffers. Mutating the message is visible through the
|
|
// SensorData. Callers must therefore keep the message alive and unchanged for as long
|
|
// as they use the result -- and must deep-copy before letting the SensorData outlive
|
|
// the subscription callback, since the ROS queue recycles the message once it
|
|
// returns.
|
|
cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
|
|
|
|
auto msg = std::make_shared<rtabmap_msgs::msg::RGBDImage>();
|
|
msg->rgb_camera_info.width = 4;
|
|
msg->rgb_camera_info.height = 4;
|
|
msg->rgb_camera_info.k = {525.0, 0.0, 2.0, 0.0, 525.0, 2.0, 0.0, 0.0, 1.0};
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb);
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg->depth);
|
|
|
|
const rtabmap::SensorData data = rgbdImageFromROS(msg);
|
|
ASSERT_FALSE(data.imageRaw().empty());
|
|
ASSERT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
|
|
|
|
// Writing through the message is observable in the SensorData: no copy was made.
|
|
msg->rgb.data[0] = 99;
|
|
EXPECT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0)[0], 99)
|
|
<< "SensorData is expected to alias the message buffer";
|
|
}
|
|
|
|
TEST(MsgConversion, toCvCopyReadsRawImages)
|
|
{
|
|
cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb);
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth);
|
|
|
|
cv_bridge::CvImagePtr rgbPtr, depthPtr;
|
|
toCvCopy(msg, rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(rgbPtr && depthPtr);
|
|
EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
|
|
EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
|
|
|
|
// The copy must be independent of the message buffer.
|
|
rgbPtr->image.at<cv::Vec3b>(0, 0) = cv::Vec3b(0, 0, 0);
|
|
EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
|
|
}
|
|
|
|
TEST(MsgConversion, toCvCopyEmptyImageYieldsEmptyPtr)
|
|
{
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
|
|
cv_bridge::CvImagePtr rgbPtr, depthPtr;
|
|
toCvCopy(msg, rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(rgbPtr && depthPtr) << "pointers must be valid even with no image";
|
|
EXPECT_TRUE(rgbPtr->image.empty());
|
|
EXPECT_TRUE(depthPtr->image.empty());
|
|
}
|
|
|
|
TEST(MsgConversion, toCvShareAliasesRawImages)
|
|
{
|
|
cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb);
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth);
|
|
|
|
cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
|
|
toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(rgbPtr && depthPtr);
|
|
ASSERT_FALSE(rgbPtr->image.empty());
|
|
EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
|
|
EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
|
|
}
|
|
|
|
/////////////////////////
|
|
// Compressed images
|
|
/////////////////////////
|
|
|
|
namespace {
|
|
|
|
/// Builds a depth image compressed the way rtabmap does it (not a jpg/png CompressedImage).
|
|
sensor_msgs::msg::CompressedImage makeRtabmapCompressedDepth(const cv::Mat & depth)
|
|
{
|
|
sensor_msgs::msg::CompressedImage msg;
|
|
msg.format = ""; // anything but "jpg" takes the rtabmap::uncompressImage path
|
|
msg.data = rtabmap::compressImage(depth, ".png");
|
|
return msg;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, toCvCopyReadsCompressedDepth)
|
|
{
|
|
const cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1234));
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
msg.depth_compressed = makeRtabmapCompressedDepth(depth);
|
|
|
|
cv_bridge::CvImagePtr rgbPtr, depthPtr;
|
|
toCvCopy(msg, rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(depthPtr);
|
|
ASSERT_FALSE(depthPtr->image.empty());
|
|
EXPECT_EQ(depthPtr->image.type(), CV_16UC1);
|
|
EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
|
EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, toCvShareReadsCompressedDepth)
|
|
{
|
|
const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.5f));
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
msg.depth_compressed = makeRtabmapCompressedDepth(depth);
|
|
|
|
cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
|
|
toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(depthPtr);
|
|
ASSERT_FALSE(depthPtr->image.empty());
|
|
EXPECT_EQ(depthPtr->image.type(), CV_32FC1);
|
|
EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_32FC1);
|
|
EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, toCvShareOnAnEmptyMessageGivesEmptyImages)
|
|
{
|
|
// Both pointers must be valid even when the message carries nothing: callers such as
|
|
// rgbdImageFromROS() dereference them unconditionally.
|
|
const rtabmap_msgs::msg::RGBDImage msg;
|
|
|
|
cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
|
|
toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(rgbPtr);
|
|
ASSERT_TRUE(depthPtr);
|
|
EXPECT_TRUE(rgbPtr->image.empty());
|
|
EXPECT_TRUE(depthPtr->image.empty());
|
|
}
|
|
|
|
TEST(MsgConversion, rgbdImageFromROSOnAnEmptyMessageIsInvalid)
|
|
{
|
|
rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
|
|
std::make_shared<rtabmap_msgs::msg::RGBDImage>();
|
|
msg->header.frame_id = "camera_link";
|
|
msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
|
|
|
|
const rtabmap::SensorData data = rgbdImageFromROS(msg);
|
|
|
|
EXPECT_FALSE(data.isValid()) << "an empty message must give empty data, not a crash";
|
|
}
|
|
|
|
TEST(MsgConversion, rgbdImageFromROSWithoutDepthKeepsTheColorImage)
|
|
{
|
|
// The depth image is optional: color plus camera info is a valid message, and the
|
|
// resolution check must not divide by the zero depth width.
|
|
rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
|
|
std::make_shared<rtabmap_msgs::msg::RGBDImage>();
|
|
msg->header.frame_id = "camera_link";
|
|
msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
|
|
cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb);
|
|
msg->rgb_camera_info.width = 8;
|
|
msg->rgb_camera_info.height = 8;
|
|
msg->rgb_camera_info.k = {525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0};
|
|
|
|
const rtabmap::SensorData data = rgbdImageFromROS(msg);
|
|
|
|
EXPECT_TRUE(data.isValid());
|
|
ASSERT_FALSE(data.imageRaw().empty());
|
|
EXPECT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
|
|
EXPECT_TRUE(data.depthRaw().empty());
|
|
ASSERT_EQ(data.cameraModels().size(), 1u);
|
|
EXPECT_NEAR(data.cameraModels()[0].fx(), 525.0, 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, toCvCopyReadsCompressedRgb)
|
|
{
|
|
const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
|
|
rtabmap_msgs::msg::RGBDImage msg;
|
|
msg.rgb_compressed.format = "png";
|
|
msg.rgb_compressed.data = rtabmap::compressImage(rgb, ".png");
|
|
|
|
cv_bridge::CvImagePtr rgbPtr, depthPtr;
|
|
toCvCopy(msg, rgbPtr, depthPtr);
|
|
|
|
ASSERT_TRUE(rgbPtr);
|
|
ASSERT_FALSE(rgbPtr->image.empty());
|
|
EXPECT_EQ(rgbPtr->image.type(), CV_8UC3);
|
|
EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
|
|
}
|
|
|
|
/////////////////////////
|
|
// SensorData: raw copies, laser scans, stereo
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, sensorDataToROSCopyRawDataCarriesImages)
|
|
{
|
|
cv::Mat K = (cv::Mat_<double>(3, 3) <<
|
|
525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0);
|
|
const rtabmap::CameraModel model("cam", cv::Size(8, 8), K, cv::Mat(), cv::Mat(), cv::Mat());
|
|
|
|
const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(2000));
|
|
rtabmap::SensorData in(rgb, depth, model, 1, 1000.0);
|
|
|
|
// Without copyRawData the raw images are not serialized...
|
|
rtabmap_msgs::msg::SensorData without;
|
|
sensorDataToROS(in, without, "base_link", /*copyRawData=*/false);
|
|
EXPECT_TRUE(without.left.data.empty());
|
|
EXPECT_TRUE(without.right.data.empty());
|
|
|
|
// ...with it, they are.
|
|
rtabmap_msgs::msg::SensorData with;
|
|
sensorDataToROS(in, with, "base_link", /*copyRawData=*/true);
|
|
ASSERT_FALSE(with.left.data.empty());
|
|
ASSERT_FALSE(with.right.data.empty());
|
|
EXPECT_EQ(with.left.encoding, sensor_msgs::image_encodings::BGR8);
|
|
EXPECT_EQ(with.right.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
|
|
|
|
const rtabmap::SensorData out = sensorDataFromROS(with);
|
|
ASSERT_FALSE(out.imageRaw().empty());
|
|
EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0);
|
|
ASSERT_FALSE(out.depthRaw().empty());
|
|
EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0);
|
|
}
|
|
|
|
TEST(MsgConversion, sensorDataLaserScanRoundTrip)
|
|
{
|
|
cv::Mat points(1, 3, CV_32FC3);
|
|
points.at<cv::Vec3f>(0, 0) = cv::Vec3f(1.0f, 0.0f, 0.0f);
|
|
points.at<cv::Vec3f>(0, 1) = cv::Vec3f(0.0f, 2.0f, 0.0f);
|
|
points.at<cv::Vec3f>(0, 2) = cv::Vec3f(0.0f, 0.0f, 3.0f);
|
|
|
|
const rtabmap::Transform localTransform(0.0f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f);
|
|
const rtabmap::LaserScan scan(points, /*maxPoints=*/100, /*maxRange=*/40.0f,
|
|
rtabmap::LaserScan::kXYZ, localTransform);
|
|
|
|
rtabmap::SensorData in;
|
|
in.setStamp(1000.0);
|
|
in.setLaserScan(scan);
|
|
|
|
rtabmap_msgs::msg::SensorData msg;
|
|
sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true);
|
|
|
|
EXPECT_EQ(msg.laser_scan_max_pts, 100);
|
|
EXPECT_FLOAT_EQ(msg.laser_scan_max_range, 40.0f);
|
|
EXPECT_EQ(msg.laser_scan_format, (int)rtabmap::LaserScan::kXYZ);
|
|
expectTransformNear(transformFromGeometryMsg(msg.laser_scan_local_transform),
|
|
localTransform, 1e-4f);
|
|
|
|
const rtabmap::SensorData out = sensorDataFromROS(msg);
|
|
const rtabmap::LaserScan & outScan = out.laserScanRaw().empty()
|
|
? out.laserScanCompressed() : out.laserScanRaw();
|
|
EXPECT_EQ(outScan.size(), scan.size());
|
|
EXPECT_EQ(outScan.maxPoints(), scan.maxPoints());
|
|
EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax());
|
|
expectTransformNear(outScan.localTransform(), localTransform, 1e-4f);
|
|
}
|
|
|
|
TEST(MsgConversion, sensorDataStereoModelRoundTrip)
|
|
{
|
|
const double fx = 525.0;
|
|
const double baseline = 0.12;
|
|
const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
|
|
|
|
const rtabmap::StereoCameraModel stereo(
|
|
fx, fx, 320.0, 240.0, baseline, localTransform, cv::Size(640, 480));
|
|
ASSERT_TRUE(stereo.isValidForProjection()) << "precondition";
|
|
|
|
rtabmap::SensorData in;
|
|
in.setStamp(1000.0);
|
|
in.setStereoImage(cv::Mat(), cv::Mat(), stereo);
|
|
|
|
rtabmap_msgs::msg::SensorData msg;
|
|
sensorDataToROS(in, msg, "base_link");
|
|
|
|
// The stereo branch fills BOTH camera infos, unlike the monocular one.
|
|
ASSERT_EQ(msg.left_camera_info.size(), 1u);
|
|
ASSERT_EQ(msg.right_camera_info.size(), 1u);
|
|
|
|
const rtabmap::SensorData out = sensorDataFromROS(msg);
|
|
ASSERT_EQ(out.stereoCameraModels().size(), 1u);
|
|
EXPECT_TRUE(out.cameraModels().empty()) << "must not be read back as monocular";
|
|
EXPECT_NEAR(out.stereoCameraModels()[0].left().fx(), fx, 1e-9);
|
|
EXPECT_NEAR(out.stereoCameraModels()[0].baseline(), baseline, 1e-6);
|
|
expectTransformNear(out.stereoCameraModels()[0].localTransform(), localTransform, 1e-4f);
|
|
}
|
|
|
|
TEST(MsgConversion, nodeWithStereoModelRoundTrip)
|
|
{
|
|
const rtabmap::StereoCameraModel stereo(
|
|
525.0, 525.0, 320.0, 240.0, 0.12,
|
|
rtabmap::Transform::getIdentity(), cv::Size(640, 480));
|
|
|
|
rtabmap::Signature in(3, 0, 1, 1000.0, "stereo_node", sampleTransform());
|
|
in.sensorData().setStereoImage(cv::Mat(), cv::Mat(), stereo);
|
|
|
|
rtabmap_msgs::msg::Node msg;
|
|
nodeToROS(in, msg);
|
|
const rtabmap::Signature out = nodeFromROS(msg);
|
|
|
|
EXPECT_EQ(out.id(), in.id());
|
|
ASSERT_EQ(out.sensorData().stereoCameraModels().size(), 1u);
|
|
EXPECT_NEAR(out.sensorData().stereoCameraModels()[0].baseline(), 0.12, 1e-6);
|
|
}
|
|
|
|
TEST(MsgConversion, infoToROSKeepsACallerSuppliedStamp)
|
|
{
|
|
// CoreWrapper stamps the message before calling infoToROS, sometimes with a
|
|
// publication time unrelated to the data. That must not be overwritten.
|
|
rtabmap::Statistics in;
|
|
in.setExtended(true);
|
|
in.setStamp(1234.5);
|
|
|
|
rtabmap_msgs::msg::Info msg;
|
|
msg.header.stamp = timestampToROS(9999.0);
|
|
msg.header.frame_id = "map";
|
|
infoToROS(in, msg);
|
|
|
|
EXPECT_NEAR(timestampFromROS(msg.header.stamp), 9999.0, 1e-6)
|
|
<< "a caller-supplied stamp must win over the statistics stamp";
|
|
EXPECT_EQ(msg.header.frame_id, "map");
|
|
}
|
|
|
|
TEST(MsgConversion, infoOdomCacheRoundTrip)
|
|
{
|
|
// Statistics carries a whole MapGraph for the odometry cache in localization mode.
|
|
std::map<int, rtabmap::Transform> poses;
|
|
poses.insert(std::make_pair(1, sampleTransform()));
|
|
poses.insert(std::make_pair(2, rtabmap::Transform(1, 2, 3, 0, 0, 0)));
|
|
|
|
std::multimap<int, rtabmap::Link> links;
|
|
links.insert(std::make_pair(1, rtabmap::Link(
|
|
1, 2, rtabmap::Link::kNeighbor, sampleTransform())));
|
|
|
|
rtabmap::Statistics in;
|
|
in.setExtended(true);
|
|
in.setOdomCachePoses(poses);
|
|
in.setOdomCacheConstraints(links);
|
|
|
|
rtabmap_msgs::msg::Info msg;
|
|
infoToROS(in, msg);
|
|
ASSERT_EQ(msg.odom_cache.poses.size(), poses.size());
|
|
ASSERT_EQ(msg.odom_cache.links.size(), links.size());
|
|
|
|
rtabmap::Statistics out;
|
|
infoFromROS(msg, out);
|
|
|
|
ASSERT_EQ(out.odomCachePoses().size(), poses.size());
|
|
expectTransformNear(out.odomCachePoses().at(1), poses.at(1));
|
|
expectTransformNear(out.odomCachePoses().at(2), poses.at(2));
|
|
EXPECT_EQ(out.odomCacheConstraints().size(), links.size());
|
|
}
|
|
|
|
/////////////////////////
|
|
// TF-based conversions
|
|
/////////////////////////
|
|
|
|
namespace {
|
|
|
|
/// A tf2 buffer needs a clock, but neither a node nor a listener: transforms can be
|
|
/// injected directly, which makes every TF-based conversion an ordinary unit test.
|
|
std::shared_ptr<tf2_ros::Buffer> makeTfBuffer()
|
|
{
|
|
std::shared_ptr<tf2_ros::Buffer> buffer =
|
|
std::make_shared<tf2_ros::Buffer>(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
|
|
// Transforms are injected synchronously before the lookups, so tell tf2 not to warn
|
|
// about waiting for a listener thread that will never exist.
|
|
buffer->setUsingDedicatedThread(true);
|
|
return buffer;
|
|
}
|
|
|
|
void addTf(tf2_ros::Buffer & buffer,
|
|
const std::string & parent, const std::string & child,
|
|
const rtabmap::Transform & t, double stamp, bool isStatic = true)
|
|
{
|
|
geometry_msgs::msg::TransformStamped msg;
|
|
msg.header.stamp = timestampToROS(stamp);
|
|
msg.header.frame_id = parent;
|
|
msg.child_frame_id = child;
|
|
transformToGeometryMsg(t, msg.transform);
|
|
ASSERT_TRUE(buffer.setTransform(msg, "unit_test", isStatic));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, getTransformReadsTheBuffer)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
|
|
const rtabmap::Transform out =
|
|
getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0);
|
|
|
|
expectTransformNear(out, baseToCamera);
|
|
}
|
|
|
|
TEST(MsgConversion, getTransformReturnsNullWhenUnknown)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
// An unrelated frame must not throw; it must come back as a null transform.
|
|
EXPECT_TRUE(getTransform("base_link", "lidar_link", timestampToROS(1000.0), *buffer, 0.0)
|
|
.isNull());
|
|
}
|
|
|
|
TEST(MsgConversion, getTransformIsInvertedByFrameOrder)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.5f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
|
|
const rtabmap::Transform forward =
|
|
getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0);
|
|
const rtabmap::Transform backward =
|
|
getTransform("camera_link", "base_link", timestampToROS(1000.0), *buffer, 0.0);
|
|
|
|
expectTransformNear(backward, forward.inverse(), 1e-4f);
|
|
}
|
|
|
|
TEST(MsgConversion, getMovingTransformMeasuresMotionBetweenStamps)
|
|
{
|
|
// base_link drives 1 m along x of odom between t=1000 and t=1001.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
|
|
|
|
// Motion of base_link from t=1000 to t=1001, seen in the fixed odom frame.
|
|
const rtabmap::Transform motion = getMovingTransform(
|
|
"base_link", "odom", timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0);
|
|
|
|
ASSERT_FALSE(motion.isNull());
|
|
EXPECT_NEAR(motion.x(), 1.0, 1e-4);
|
|
EXPECT_NEAR(motion.y(), 0.0, 1e-4);
|
|
EXPECT_NEAR(motion.z(), 0.0, 1e-4);
|
|
}
|
|
|
|
TEST(MsgConversion, getMovingTransformInterpolatesBetweenStamps)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
|
|
|
|
// Halfway through, so half the motion.
|
|
const rtabmap::Transform half = getMovingTransform(
|
|
"base_link", "odom", timestampToROS(1000.0), timestampToROS(1000.5), *buffer, 0.0);
|
|
|
|
ASSERT_FALSE(half.isNull());
|
|
EXPECT_NEAR(half.x(), 0.5, 1e-4);
|
|
}
|
|
|
|
TEST(MsgConversion, getMovingTransformIsNullWithoutAFixedFrame)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform::getIdentity(), 1000.0, false);
|
|
|
|
EXPECT_TRUE(getMovingTransform("base_link", "map",
|
|
timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0).isNull());
|
|
}
|
|
|
|
TEST(MsgConversion, convertScanMsgProducesALaserScan)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0);
|
|
|
|
sensor_msgs::msg::LaserScan msg;
|
|
msg.header.stamp = timestampToROS(1000.0);
|
|
msg.header.frame_id = "laser";
|
|
msg.angle_min = -1.0f;
|
|
msg.angle_max = 1.0f;
|
|
msg.angle_increment = 0.1f;
|
|
msg.time_increment = 0.0f;
|
|
msg.range_min = 0.1f;
|
|
msg.range_max = 30.0f;
|
|
msg.ranges.assign(21, 5.0f);
|
|
|
|
rtabmap::LaserScan scan;
|
|
ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
|
|
EXPECT_FALSE(scan.empty());
|
|
EXPECT_EQ(scan.size(), (int)msg.ranges.size());
|
|
expectTransformNear(scan.localTransform(), baseToLaser, 1e-4f);
|
|
}
|
|
|
|
TEST(MsgConversion, convertScanMsgRejectsMalformedScans)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
sensor_msgs::msg::LaserScan base;
|
|
base.header.stamp = timestampToROS(1000.0);
|
|
base.header.frame_id = "laser";
|
|
base.angle_min = -1.0f;
|
|
base.angle_max = 1.0f;
|
|
base.angle_increment = 0.1f;
|
|
base.range_min = 0.1f;
|
|
base.range_max = 30.0f;
|
|
base.ranges.assign(21, 5.0f);
|
|
|
|
rtabmap::LaserScan scan;
|
|
|
|
sensor_msgs::msg::LaserScan zeroIncrement = base;
|
|
zeroIncrement.angle_increment = 0.0f;
|
|
EXPECT_FALSE(convertScanMsg(zeroIncrement, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0)) << "angle_increment of 0 would divide by zero";
|
|
|
|
sensor_msgs::msg::LaserScan invertedRange = base;
|
|
invertedRange.range_min = 40.0f;
|
|
EXPECT_FALSE(convertScanMsg(invertedRange, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0)) << "range_min > range_max";
|
|
|
|
sensor_msgs::msg::LaserScan invertedAngle = base;
|
|
invertedAngle.angle_min = 1.0f;
|
|
invertedAngle.angle_max = -1.0f;
|
|
EXPECT_FALSE(convertScanMsg(invertedAngle, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0)) << "positive increment with angle_max < angle_min";
|
|
}
|
|
|
|
TEST(MsgConversion, convertScanMsgFailsWithoutTf)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer(); // empty
|
|
|
|
sensor_msgs::msg::LaserScan msg;
|
|
msg.header.stamp = timestampToROS(1000.0);
|
|
msg.header.frame_id = "laser";
|
|
msg.angle_min = -1.0f;
|
|
msg.angle_max = 1.0f;
|
|
msg.angle_increment = 0.1f;
|
|
msg.range_min = 0.1f;
|
|
msg.range_max = 30.0f;
|
|
msg.ranges.assign(21, 5.0f);
|
|
|
|
rtabmap::LaserScan scan;
|
|
EXPECT_FALSE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
}
|
|
|
|
TEST(MsgConversion, convertScan3dMsgKeepsLocalTransformAndLimits)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToLidar(0.0f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "lidar", baseToLidar, 1000.0);
|
|
|
|
sensor_msgs::msg::PointCloud2 msg =
|
|
makeXYZCloud({{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {3.0f, 0.0f, 0.0f}});
|
|
msg.header.stamp = timestampToROS(1000.0);
|
|
msg.header.frame_id = "lidar";
|
|
|
|
rtabmap::LaserScan scan;
|
|
ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
|
|
EXPECT_EQ(scan.size(), 3);
|
|
expectTransformNear(scan.localTransform(), baseToLidar, 1e-4f);
|
|
EXPECT_EQ(scan.rangeMax(), 0.0f) << "no max range requested";
|
|
|
|
rtabmap::LaserScan limited;
|
|
ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
|
|
limited, *buffer, 0.0, /*maxPoints=*/10, /*maxRange=*/2.5f));
|
|
EXPECT_EQ(limited.maxPoints(), 10);
|
|
EXPECT_FLOAT_EQ(limited.rangeMax(), 2.5f);
|
|
}
|
|
|
|
TEST(MsgConversion, convertScan3dMsgFailsWithoutTf)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer(); // empty
|
|
|
|
sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
|
|
msg.header.stamp = timestampToROS(1000.0);
|
|
msg.header.frame_id = "lidar";
|
|
|
|
rtabmap::LaserScan scan;
|
|
EXPECT_FALSE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
}
|
|
|
|
TEST(MsgConversion, landmarksFromROSAppliesTfAndDefaultVariance)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
|
|
geometry_msgs::msg::PoseWithCovarianceStamped tag;
|
|
tag.header.stamp = timestampToROS(1000.0);
|
|
tag.header.frame_id = "camera_link";
|
|
tag.pose.pose.position.x = 2.0; // 2 m in front of the camera
|
|
tag.pose.pose.orientation.w = 1.0;
|
|
// covariance left at zero -> the defaults must be substituted
|
|
|
|
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
|
|
tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f)));
|
|
|
|
const rtabmap::Landmarks landmarks = landmarksFromROS(
|
|
tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0,
|
|
/*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02);
|
|
|
|
ASSERT_EQ(landmarks.size(), 1u);
|
|
ASSERT_TRUE(landmarks.find(7) != landmarks.end());
|
|
|
|
// The tag pose must come back in base_link: 0.5 (base->camera) + 2.0 (camera->tag).
|
|
EXPECT_NEAR(landmarks.at(7).pose().x(), 2.5, 1e-4);
|
|
|
|
const cv::Mat cov = landmarks.at(7).covariance();
|
|
ASSERT_EQ(cov.rows, 6);
|
|
EXPECT_NEAR(cov.at<double>(0,0), 0.01, 1e-9) << "linear default";
|
|
EXPECT_NEAR(cov.at<double>(3,3), 0.02, 1e-9) << "angular default";
|
|
}
|
|
|
|
TEST(MsgConversion, landmarksFromROSCorrectsForOdometryMotion)
|
|
{
|
|
// The tag is seen 1 s after the odometry stamp, during which the robot drives 1 m.
|
|
// landmarksFromROS must fold that motion in, otherwise the landmark is placed where
|
|
// the robot would have seen it had it not moved.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
|
|
|
|
geometry_msgs::msg::PoseWithCovarianceStamped tag;
|
|
tag.header.stamp = timestampToROS(1001.0); // observed at t=1001
|
|
tag.header.frame_id = "camera_link";
|
|
tag.pose.pose.position.x = 2.0;
|
|
tag.pose.pose.orientation.w = 1.0;
|
|
|
|
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
|
|
tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f)));
|
|
|
|
// odomStamp is 1000, one second BEFORE the observation.
|
|
const rtabmap::Landmarks corrected = landmarksFromROS(
|
|
tags, "base_link", "odom", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
|
|
|
|
ASSERT_EQ(corrected.size(), 1u);
|
|
// 0.5 (base->camera) + 2.0 (camera->tag) + 1.0 (odometry motion since odomStamp).
|
|
EXPECT_NEAR(corrected.at(7).pose().x(), 3.5, 1e-3);
|
|
|
|
// Without an odom frame the correction cannot be looked up, and the landmark stays
|
|
// in the frame at the observation stamp.
|
|
const rtabmap::Landmarks uncorrected = landmarksFromROS(
|
|
tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
|
|
ASSERT_EQ(uncorrected.size(), 1u);
|
|
EXPECT_NEAR(uncorrected.at(7).pose().x(), 2.5, 1e-3);
|
|
}
|
|
|
|
TEST(MsgConversion, landmarksFromROSKeepsProvidedCovariance)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
geometry_msgs::msg::PoseWithCovarianceStamped tag;
|
|
tag.header.stamp = timestampToROS(1000.0);
|
|
tag.header.frame_id = "camera_link";
|
|
tag.pose.pose.position.x = 1.0;
|
|
tag.pose.pose.orientation.w = 1.0;
|
|
for(size_t i=0; i<6; ++i)
|
|
{
|
|
tag.pose.covariance[i*6 + i] = 0.5; // a real, finite covariance
|
|
}
|
|
|
|
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
|
|
tags.insert(std::make_pair(1, std::make_pair(tag, 0.1f)));
|
|
|
|
const rtabmap::Landmarks landmarks = landmarksFromROS(
|
|
tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0,
|
|
/*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02);
|
|
|
|
ASSERT_EQ(landmarks.size(), 1u);
|
|
EXPECT_NEAR(landmarks.at(1).covariance().at<double>(0,0), 0.5, 1e-9)
|
|
<< "a provided covariance must not be replaced by the default";
|
|
EXPECT_NEAR(landmarks.at(1).covariance().at<double>(3,3), 0.5, 1e-9);
|
|
}
|
|
|
|
TEST(MsgConversion, landmarksFromROSRejectsNonPositiveIds)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
geometry_msgs::msg::PoseWithCovarianceStamped tag;
|
|
tag.header.stamp = timestampToROS(1000.0);
|
|
tag.header.frame_id = "camera_link";
|
|
tag.pose.pose.orientation.w = 1.0;
|
|
|
|
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
|
|
tags.insert(std::make_pair(0, std::make_pair(tag, 0.1f)));
|
|
tags.insert(std::make_pair(-3, std::make_pair(tag, 0.1f)));
|
|
tags.insert(std::make_pair(5, std::make_pair(tag, 0.1f)));
|
|
|
|
const rtabmap::Landmarks landmarks = landmarksFromROS(
|
|
tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
|
|
|
|
EXPECT_EQ(landmarks.size(), 1u) << "ids <= 0 must be dropped";
|
|
EXPECT_TRUE(landmarks.find(5) != landmarks.end());
|
|
}
|
|
|
|
void expectTfDeskewRecoversWall(bool slerp)
|
|
{
|
|
SCOPED_TRACE(slerp ? "slerp=true" : "slerp=false");
|
|
|
|
// base_link advances 0.1 m along odom over the sweep -- the same motion the constant
|
|
// velocity tests apply at 1 m/s. With slerp the correction is interpolated between
|
|
// the two end poses; without it, every sample gets its own TF lookup.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
|
|
addTf(*buffer, "odom", "base_link",
|
|
rtabmap::Transform(float(kSpeed * kScanSpan), 0, 0, 0, 0, 0),
|
|
1000.0 + kScanSpan, false);
|
|
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
|
|
in.header.frame_id = "base_link";
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
ASSERT_TRUE(deskew(in, out, "odom", *buffer, 0.0, slerp));
|
|
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
|
|
<< "sample " << i;
|
|
}
|
|
}
|
|
|
|
TEST(MsgConversion, deskewWithTfBufferSlerp)
|
|
{
|
|
expectTfDeskewRecoversWall(/*slerp=*/true);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewWithTfBufferPerPointLookup)
|
|
{
|
|
// slerp=false takes a completely different path: a getMovingTransform() per sample
|
|
// instead of one interpolation between the sweep's end poses.
|
|
expectTfDeskewRecoversWall(/*slerp=*/false);
|
|
}
|
|
|
|
TEST(MsgConversion, deskewSlerpLinearizesNonLinearMotion)
|
|
{
|
|
// A piecewise-linear trajectory: the robot covers most of the sweep's distance in the
|
|
// first half, then nearly stops. Both the TF buffer and the skew of the input cloud
|
|
// are generated from this same motion, so the true answer is unambiguous: a correct
|
|
// deskew must recover the flat wall.
|
|
const double kneeTime = kScanSpan / 2.0;
|
|
const double kneeX = 0.09; // vs 0.0495 if it were linear
|
|
const double endX = kSpeed * kScanSpan; // 0.099
|
|
// Matches how tf2 interpolates between consecutive samples.
|
|
auto travelled = [&](double elapsed) {
|
|
return elapsed <= kneeTime
|
|
? kneeX * (elapsed / kneeTime)
|
|
: kneeX + (endX - kneeX) * ((elapsed - kneeTime) / (kScanSpan - kneeTime));
|
|
};
|
|
|
|
auto buildBuffer = [&]() {
|
|
std::shared_ptr<tf2_ros::Buffer> b = makeTfBuffer();
|
|
geometry_msgs::msg::TransformStamped m;
|
|
m.header.frame_id = "odom";
|
|
m.child_frame_id = "base_link";
|
|
m.transform.rotation.w = 1.0;
|
|
for(double elapsed : {0.0, kneeTime, kScanSpan})
|
|
{
|
|
m.header.stamp = timestampToROS(1000.0 + elapsed);
|
|
m.transform.translation.x = travelled(elapsed);
|
|
b->setTransform(m, "unit_test", false);
|
|
}
|
|
return b;
|
|
};
|
|
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
|
|
1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t", false, travelled);
|
|
in.header.frame_id = "base_link";
|
|
|
|
sensor_msgs::msg::PointCloud2 slerped, perPoint;
|
|
const std::shared_ptr<tf2_ros::Buffer> b1 = buildBuffer();
|
|
const std::shared_ptr<tf2_ros::Buffer> b2 = buildBuffer();
|
|
ASSERT_TRUE(deskew(in, slerped, "odom", *b1, 0.0, /*slerp=*/true));
|
|
ASSERT_TRUE(deskew(in, perPoint, "odom", *b2, 0.0, /*slerp=*/false));
|
|
|
|
// Per-point lookups follow the real motion, so they reconstruct the wall exactly.
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
EXPECT_NEAR(readWallX(perPoint, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
|
|
<< "slerp=false must be exact, sample " << i;
|
|
}
|
|
|
|
// slerp only reads the sweep's two end poses, so it straight-lines through the knee
|
|
// and leaves a visible residual in the middle of the scan.
|
|
double worst = 0.0;
|
|
for(size_t i=0; i<kScanPoints; ++i)
|
|
{
|
|
worst = std::max(worst,
|
|
std::abs(double(readWallX(slerped, i, 0, kTimeOnColumns)) - kWallDistance));
|
|
}
|
|
EXPECT_GT(worst, 1e-2) << "slerp must show the error of linearizing the motion";
|
|
}
|
|
|
|
TEST(MsgConversion, deskewWithTfBufferFailsWithoutTf)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer(); // empty
|
|
|
|
sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
|
|
in.header.frame_id = "base_link";
|
|
|
|
sensor_msgs::msg::PointCloud2 out;
|
|
EXPECT_FALSE(deskew(in, out, "odom", *buffer, 0.0, true));
|
|
}
|
|
|
|
/////////////////////////
|
|
// convertRGBDMsgs / convertStereoMsg
|
|
/////////////////////////
|
|
|
|
namespace {
|
|
|
|
/// A rectified pinhole CameraInfo. tx is P(0,3): 0 for the left/depth camera, and
|
|
/// -fx*baseline for the right camera of a stereo pair.
|
|
sensor_msgs::msg::CameraInfo makeCameraInfo(
|
|
const std::string & frameId, double stamp, int width, int height,
|
|
double tx = 0.0, double fx = 100.0)
|
|
{
|
|
sensor_msgs::msg::CameraInfo info;
|
|
info.header.stamp = timestampToROS(stamp);
|
|
info.header.frame_id = frameId;
|
|
info.width = width;
|
|
info.height = height;
|
|
info.distortion_model = "plumb_bob";
|
|
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
|
|
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
|
|
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
|
info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
|
return info;
|
|
}
|
|
|
|
cv_bridge::CvImageConstPtr makeImage(
|
|
const std::string & frameId, double stamp,
|
|
const cv::Mat & image, const std::string & encoding)
|
|
{
|
|
std_msgs::msg::Header header;
|
|
header.stamp = timestampToROS(stamp);
|
|
header.frame_id = frameId;
|
|
return std::make_shared<cv_bridge::CvImage>(header, encoding, image);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsSingleCamera)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
|
|
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths =
|
|
{makeImage("camera_link", 1000.0, depthImage, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1000.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels,
|
|
*buffer, 0.0, /*alreadyRectifiedImages=*/true));
|
|
|
|
EXPECT_TRUE(stereoModels.empty()) << "a depth image must not produce a stereo model";
|
|
ASSERT_EQ(models.size(), 1u);
|
|
EXPECT_NEAR(models[0].fx(), 100.0, 1e-9);
|
|
expectTransformNear(models[0].localTransform(), baseToCamera, 1e-4f);
|
|
|
|
ASSERT_EQ(rgb.cols, 8);
|
|
ASSERT_EQ(rgb.rows, 8);
|
|
EXPECT_EQ(depth.type(), CV_16UC1);
|
|
EXPECT_EQ(depth.at<unsigned short>(0, 0), 1500);
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsMultiCameraSideBySide)
|
|
{
|
|
// Two cameras are concatenated horizontally into one wide image, one model each.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
|
|
addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
|
|
|
|
const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0));
|
|
const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0));
|
|
const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000));
|
|
const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000));
|
|
|
|
const std::vector<cv_bridge::CvImageConstPtr> images = {
|
|
makeImage("cam0", 1000.0, rgb0, "bgr8"),
|
|
makeImage("cam1", 1000.0, rgb1, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths = {
|
|
makeImage("cam0", 1000.0, depth0, "16UC1"),
|
|
makeImage("cam1", 1000.0, depth1, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos = {
|
|
makeCameraInfo("cam0", 1000.0, 8, 8),
|
|
makeCameraInfo("cam1", 1000.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels,
|
|
*buffer, 0.0, true));
|
|
|
|
ASSERT_EQ(models.size(), 2u);
|
|
EXPECT_EQ(rgb.cols, 16) << "the two 8-wide images must be side by side";
|
|
EXPECT_EQ(rgb.rows, 8);
|
|
EXPECT_EQ(depth.cols, 16);
|
|
|
|
// Each half keeps its own camera's data.
|
|
EXPECT_EQ(depth.at<unsigned short>(0, 0), 1000);
|
|
EXPECT_EQ(depth.at<unsigned short>(0, 8), 2000);
|
|
EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-4);
|
|
EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-4);
|
|
}
|
|
|
|
namespace {
|
|
|
|
/// base_link sits at odom origin at t=1000 and 1 m along x at t=1001.
|
|
void addOdomMotion(tf2_ros::Buffer & buffer)
|
|
{
|
|
geometry_msgs::msg::TransformStamped m;
|
|
m.header.frame_id = "odom";
|
|
m.child_frame_id = "base_link";
|
|
m.transform.rotation.w = 1.0;
|
|
m.header.stamp = timestampToROS(1000.0);
|
|
m.transform.translation.x = 0.0;
|
|
ASSERT_TRUE(buffer.setTransform(m, "unit_test", false));
|
|
m.header.stamp = timestampToROS(1001.0);
|
|
m.transform.translation.x = 1.0;
|
|
ASSERT_TRUE(buffer.setTransform(m, "unit_test", false));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp)
|
|
{
|
|
// The image is captured at t=1001 but must be expressed relative to the base frame
|
|
// at odomStamp=1000, one meter back.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
|
|
addOdomMotion(*buffer);
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1001.0, rgbImage, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths =
|
|
{makeImage("camera_link", 1001.0, depthImage, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1001.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> corrected;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
|
|
timestampToROS(1000.0), rgb, depth, corrected, stereoModels, *buffer, 0.0, true));
|
|
ASSERT_EQ(corrected.size(), 1u);
|
|
EXPECT_NEAR(corrected[0].localTransform().x(), 1.1, 1e-3)
|
|
<< "0.1 base->camera plus 1.0 of odometry motion";
|
|
|
|
// Without an odom frame the motion is not folded in.
|
|
std::vector<rtabmap::CameraModel> uncorrected;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels2;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, uncorrected, stereoModels2, *buffer, 0.0, true));
|
|
ASSERT_EQ(uncorrected.size(), 1u);
|
|
EXPECT_NEAR(uncorrected[0].localTransform().x(), 0.1, 1e-3);
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsSyncsEachCameraAtItsOwnStamp)
|
|
{
|
|
// Two cameras captured 1 s apart, on a robot moving 1 m/s along x. Each must be
|
|
// corrected by its OWN elapsed motion, not by a single shared one.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
|
|
addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
|
|
addOdomMotion(*buffer); // x=0 @1000, x=1 @1001
|
|
geometry_msgs::msg::TransformStamped m; // extend to x=2 @1002
|
|
m.header.frame_id = "odom";
|
|
m.child_frame_id = "base_link";
|
|
m.transform.rotation.w = 1.0;
|
|
m.header.stamp = timestampToROS(1002.0);
|
|
m.transform.translation.x = 2.0;
|
|
ASSERT_TRUE(buffer->setTransform(m, "unit_test", false));
|
|
|
|
const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0));
|
|
const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0));
|
|
const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000));
|
|
const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000));
|
|
|
|
const std::vector<cv_bridge::CvImageConstPtr> images = {
|
|
makeImage("cam0", 1001.0, rgb0, "bgr8"),
|
|
makeImage("cam1", 1002.0, rgb1, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths = {
|
|
makeImage("cam0", 1001.0, depth0, "16UC1"),
|
|
makeImage("cam1", 1002.0, depth1, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos = {
|
|
makeCameraInfo("cam0", 1001.0, 8, 8),
|
|
makeCameraInfo("cam1", 1002.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
|
|
|
|
ASSERT_EQ(models.size(), 2u);
|
|
// cam0 is 1 s after odomStamp, cam1 is 2 s after.
|
|
EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) << "0.1 + 1.0 of motion";
|
|
EXPECT_NEAR(models[1].localTransform().x(), 2.1, 1e-3) << "0.1 + 2.0 of motion";
|
|
// The corrections must differ, which is the whole point of per-camera stamps.
|
|
EXPECT_GT(models[1].localTransform().x() - models[0].localTransform().x(), 0.5);
|
|
// The lateral offsets are untouched by a purely forward motion.
|
|
EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-3);
|
|
EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-3);
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer)
|
|
{
|
|
// The RGB and depth stamps of a camera are assumed to be equal. This pins the
|
|
// tie-break for when they are not: the depth stamp prevails, since it is the one the
|
|
// geometry is synchronized to. Not a behavior to rely on -- a camera whose two
|
|
// stamps disagree is already outside the contract.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0);
|
|
addOdomMotion(*buffer);
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
|
|
|
|
// RGB stamped at odomStamp (no motion), depth stamped 1 s later (1 m of motion).
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths =
|
|
{makeImage("camera_link", 1001.0, depthImage, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1000.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
|
|
|
|
ASSERT_EQ(models.size(), 1u);
|
|
EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3)
|
|
<< "the depth stamp (1001) prevails over the rgb stamp (1000)";
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsMultiStereoBuildsOneModelPerPair)
|
|
{
|
|
// mono8 "right" images make convertRGBDMsgs take the stereo branch and produce
|
|
// StereoCameraModels instead of CameraModels. The odometry sync is not re-tested
|
|
// here: it happens in the shared loop before the depth/stereo split, so
|
|
// convertRGBDMsgsSyncsEachCameraAtItsOwnStamp already covers it for both.
|
|
const double fx = 100.0;
|
|
const double baseline0 = 0.15;
|
|
const double baseline1 = 0.20;
|
|
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "left0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
|
|
addTf(*buffer, "base_link", "left1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
|
|
|
|
const cv::Mat left0(8, 8, CV_8UC1, cv::Scalar(40));
|
|
const cv::Mat left1(8, 8, CV_8UC1, cv::Scalar(60));
|
|
const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(50));
|
|
|
|
const std::vector<cv_bridge::CvImageConstPtr> images = {
|
|
makeImage("left0", 1000.0, left0, "mono8"),
|
|
makeImage("left1", 1000.0, left1, "mono8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> rights = {
|
|
makeImage("right0", 1000.0, right, "mono8"),
|
|
makeImage("right1", 1000.0, right, "mono8")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> leftInfos = {
|
|
makeCameraInfo("left0", 1000.0, 8, 8, 0.0, fx),
|
|
makeCameraInfo("left1", 1000.0, 8, 8, 0.0, fx)};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> rightInfos = {
|
|
makeCameraInfo("right0", 1000.0, 8, 8, -fx*baseline0, fx),
|
|
makeCameraInfo("right1", 1000.0, 8, 8, -fx*baseline1, fx)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, rights, leftInfos, rightInfos, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
|
|
|
|
EXPECT_TRUE(models.empty()) << "mono8 right images must give stereo models";
|
|
ASSERT_EQ(stereoModels.size(), 2u);
|
|
|
|
// Each pair keeps its own baseline and its own local transform.
|
|
EXPECT_NEAR(stereoModels[0].baseline(), baseline0, 1e-6);
|
|
EXPECT_NEAR(stereoModels[1].baseline(), baseline1, 1e-6);
|
|
EXPECT_NEAR(stereoModels[0].localTransform().y(), 0.1, 1e-3);
|
|
EXPECT_NEAR(stereoModels[1].localTransform().y(), -0.1, 1e-3);
|
|
|
|
// The two left images are laid out side by side, as in the RGB-D case.
|
|
EXPECT_EQ(rgb.cols, 16);
|
|
EXPECT_EQ(depth.cols, 16);
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsSurvivesAFailedOdomLookup)
|
|
{
|
|
// A missing odom frame must only warn: the data is still converted, uncorrected.
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1001.0);
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1001.0, rgbImage, "bgr8")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1001.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "odom",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true))
|
|
<< "a failed odometry correction must not be fatal";
|
|
ASSERT_EQ(models.size(), 1u);
|
|
EXPECT_NEAR(models[0].localTransform().x(), 0.1, 1e-3) << "left uncorrected";
|
|
}
|
|
|
|
TEST(MsgConversion, convertStereoMsgSyncsToOdomStamp)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "left_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1000.0);
|
|
addOdomMotion(*buffer);
|
|
|
|
const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(40));
|
|
|
|
cv::Mat left, right;
|
|
rtabmap::StereoCameraModel model;
|
|
ASSERT_TRUE(convertStereoMsg(
|
|
makeImage("left_link", 1001.0, mono, "mono8"),
|
|
makeImage("right_link", 1001.0, mono, "mono8"),
|
|
makeCameraInfo("left_link", 1001.0, 8, 8, 0.0),
|
|
makeCameraInfo("right_link", 1001.0, 8, 8, -15.0),
|
|
"base_link", "odom", timestampToROS(1000.0),
|
|
left, right, model, *buffer, 0.0, true));
|
|
|
|
EXPECT_NEAR(model.localTransform().x(), 1.1, 1e-3);
|
|
}
|
|
|
|
TEST(MsgConversion, convertScan3dMsgSyncsToOdomStamp)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "lidar", rtabmap::Transform(0.0f, 0, 0.5f, 0, 0, 0), 1000.0);
|
|
addOdomMotion(*buffer);
|
|
|
|
sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
|
|
msg.header.stamp = timestampToROS(1001.0);
|
|
msg.header.frame_id = "lidar";
|
|
|
|
rtabmap::LaserScan scan;
|
|
ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "odom", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
|
|
EXPECT_NEAR(scan.localTransform().x(), 1.0, 1e-3) << "0.0 base->lidar plus 1.0 motion";
|
|
EXPECT_NEAR(scan.localTransform().z(), 0.5, 1e-3);
|
|
}
|
|
|
|
TEST(MsgConversion, convertScanMsgSyncsToOdomStamp)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "laser", rtabmap::Transform(0.2f, 0, 0.1f, 0, 0, 0), 1000.0);
|
|
addOdomMotion(*buffer);
|
|
|
|
sensor_msgs::msg::LaserScan msg;
|
|
msg.header.stamp = timestampToROS(1001.0);
|
|
msg.header.frame_id = "laser";
|
|
msg.angle_min = -1.0f;
|
|
msg.angle_max = 1.0f;
|
|
msg.angle_increment = 0.1f;
|
|
msg.time_increment = 0.0f;
|
|
msg.range_min = 0.1f;
|
|
msg.range_max = 30.0f;
|
|
msg.ranges.assign(21, 5.0f);
|
|
|
|
rtabmap::LaserScan scan;
|
|
ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0),
|
|
scan, *buffer, 0.0));
|
|
|
|
EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion";
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
// 32FC1 is a valid depth encoding but not a valid rgb/left one.
|
|
const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f));
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1000.0, bad, "32FC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1000.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels,
|
|
*buffer, 0.0, true));
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsFailsWithoutTf)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer(); // empty
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1000.0, 8, 8)};
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels,
|
|
*buffer, 0.0, true));
|
|
}
|
|
|
|
TEST(MsgConversion, convertRGBDMsgsCarriesLocalFeatures)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
|
|
const std::vector<cv_bridge::CvImageConstPtr> images =
|
|
{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
|
|
const std::vector<cv_bridge::CvImageConstPtr> depths =
|
|
{makeImage("camera_link", 1000.0, depthImage, "16UC1")};
|
|
const std::vector<sensor_msgs::msg::CameraInfo> infos =
|
|
{makeCameraInfo("camera_link", 1000.0, 8, 8)};
|
|
|
|
std::vector<rtabmap_msgs::msg::KeyPoint> kptMsgs(2);
|
|
kptMsgs[0].pt.x = 1.0f; kptMsgs[0].pt.y = 2.0f; kptMsgs[0].size = 7.0f;
|
|
kptMsgs[1].pt.x = 3.0f; kptMsgs[1].pt.y = 4.0f; kptMsgs[1].size = 7.0f;
|
|
std::vector<rtabmap_msgs::msg::Point3f> ptMsgs(2);
|
|
ptMsgs[0].x = 1.0f; ptMsgs[1].x = 2.0f;
|
|
cv::Mat descriptors = cv::Mat::ones(2, 4, CV_32FC1);
|
|
|
|
std::vector<cv::KeyPoint> outKpts;
|
|
std::vector<cv::Point3f> outPts;
|
|
cv::Mat outDescriptors;
|
|
|
|
cv::Mat rgb, depth;
|
|
std::vector<rtabmap::CameraModel> models;
|
|
std::vector<rtabmap::StereoCameraModel> stereoModels;
|
|
ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
|
|
timestampToROS(1000.0), rgb, depth, models, stereoModels,
|
|
*buffer, 0.0, true,
|
|
{kptMsgs}, {ptMsgs}, {descriptors},
|
|
&outKpts, &outPts, &outDescriptors));
|
|
|
|
ASSERT_EQ(outKpts.size(), 2u);
|
|
EXPECT_FLOAT_EQ(outKpts[0].pt.x, 1.0f);
|
|
EXPECT_FLOAT_EQ(outKpts[1].pt.x, 3.0f);
|
|
ASSERT_EQ(outPts.size(), 2u);
|
|
EXPECT_FLOAT_EQ(outPts[1].x, 2.0f);
|
|
EXPECT_EQ(outDescriptors.rows, 2);
|
|
}
|
|
|
|
TEST(MsgConversion, convertStereoMsgProducesAStereoModel)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
const rtabmap::Transform baseToLeft(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
|
|
addTf(*buffer, "base_link", "left_link", baseToLeft, 1000.0);
|
|
|
|
const double fx = 100.0;
|
|
const double baseline = 0.15;
|
|
const cv::Mat leftImage(8, 8, CV_8UC1, cv::Scalar(40));
|
|
const cv::Mat rightImage(8, 8, CV_8UC1, cv::Scalar(50));
|
|
|
|
cv::Mat left, right;
|
|
rtabmap::StereoCameraModel model;
|
|
ASSERT_TRUE(convertStereoMsg(
|
|
makeImage("left_link", 1000.0, leftImage, "mono8"),
|
|
makeImage("right_link", 1000.0, rightImage, "mono8"),
|
|
makeCameraInfo("left_link", 1000.0, 8, 8, /*tx=*/0.0, fx),
|
|
makeCameraInfo("right_link", 1000.0, 8, 8, /*tx=*/-fx*baseline, fx),
|
|
"base_link", "", timestampToROS(1000.0),
|
|
left, right, model, *buffer, 0.0, /*alreadyRectified=*/true));
|
|
|
|
EXPECT_NEAR(model.baseline(), baseline, 1e-6);
|
|
EXPECT_NEAR(model.left().fx(), fx, 1e-9);
|
|
expectTransformNear(model.localTransform(), baseToLeft, 1e-4f);
|
|
|
|
ASSERT_EQ(left.type(), CV_8UC1);
|
|
ASSERT_EQ(right.type(), CV_8UC1);
|
|
EXPECT_EQ(left.at<unsigned char>(0, 0), 40);
|
|
EXPECT_EQ(right.at<unsigned char>(0, 0), 50);
|
|
}
|
|
|
|
TEST(MsgConversion, convertStereoMsgConvertsColorToMono)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
const cv::Mat color(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
|
|
const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
|
|
|
|
cv::Mat left, right;
|
|
rtabmap::StereoCameraModel model;
|
|
ASSERT_TRUE(convertStereoMsg(
|
|
makeImage("left_link", 1000.0, color, "bgr8"),
|
|
makeImage("right_link", 1000.0, mono, "mono8"),
|
|
makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
|
|
makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
|
|
"base_link", "", timestampToROS(1000.0),
|
|
left, right, model, *buffer, 0.0, true));
|
|
|
|
// The left image is kept in color; the right is always reduced to mono.
|
|
EXPECT_EQ(left.type(), CV_8UC3);
|
|
EXPECT_EQ(right.type(), CV_8UC1);
|
|
}
|
|
|
|
TEST(MsgConversion, convertStereoMsgRejectsBadEncoding)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
|
|
addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0);
|
|
|
|
const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f));
|
|
const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
|
|
|
|
cv::Mat left, right;
|
|
rtabmap::StereoCameraModel model;
|
|
EXPECT_FALSE(convertStereoMsg(
|
|
makeImage("left_link", 1000.0, bad, "32FC1"),
|
|
makeImage("right_link", 1000.0, mono, "mono8"),
|
|
makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
|
|
makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
|
|
"base_link", "", timestampToROS(1000.0),
|
|
left, right, model, *buffer, 0.0, true));
|
|
}
|
|
|
|
TEST(MsgConversion, convertStereoMsgFailsWithoutTf)
|
|
{
|
|
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer(); // empty
|
|
const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
|
|
|
|
cv::Mat left, right;
|
|
rtabmap::StereoCameraModel model;
|
|
EXPECT_FALSE(convertStereoMsg(
|
|
makeImage("left_link", 1000.0, mono, "mono8"),
|
|
makeImage("right_link", 1000.0, mono, "mono8"),
|
|
makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
|
|
makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
|
|
"base_link", "", timestampToROS(1000.0),
|
|
left, right, model, *buffer, 0.0, true));
|
|
}
|
|
|
|
/////////////////////////
|
|
// IMU
|
|
/////////////////////////
|
|
|
|
TEST(MsgConversion, imuRoundTrip)
|
|
{
|
|
sensor_msgs::msg::Imu in;
|
|
in.orientation.x = 0.0;
|
|
in.orientation.y = 0.0;
|
|
in.orientation.z = 0.0;
|
|
in.orientation.w = 1.0;
|
|
in.angular_velocity.x = 0.1;
|
|
in.angular_velocity.y = 0.2;
|
|
in.angular_velocity.z = 0.3;
|
|
in.linear_acceleration.x = 1.0;
|
|
in.linear_acceleration.y = 2.0;
|
|
in.linear_acceleration.z = 9.81;
|
|
for(size_t i=0; i<9; ++i)
|
|
{
|
|
in.orientation_covariance[i] = 0.01 * (i + 1);
|
|
in.angular_velocity_covariance[i] = 0.02 * (i + 1);
|
|
in.linear_acceleration_covariance[i] = 0.03 * (i + 1);
|
|
}
|
|
|
|
const rtabmap::IMU imu = imuFromROS(in, rtabmap::Transform::getIdentity());
|
|
|
|
sensor_msgs::msg::Imu out;
|
|
imuToROS(imu, out);
|
|
|
|
EXPECT_DOUBLE_EQ(out.orientation.w, in.orientation.w);
|
|
EXPECT_DOUBLE_EQ(out.angular_velocity.x, in.angular_velocity.x);
|
|
EXPECT_DOUBLE_EQ(out.angular_velocity.y, in.angular_velocity.y);
|
|
EXPECT_DOUBLE_EQ(out.angular_velocity.z, in.angular_velocity.z);
|
|
EXPECT_DOUBLE_EQ(out.linear_acceleration.x, in.linear_acceleration.x);
|
|
EXPECT_DOUBLE_EQ(out.linear_acceleration.y, in.linear_acceleration.y);
|
|
EXPECT_DOUBLE_EQ(out.linear_acceleration.z, in.linear_acceleration.z);
|
|
for(size_t i=0; i<9; ++i)
|
|
{
|
|
EXPECT_DOUBLE_EQ(out.orientation_covariance[i], in.orientation_covariance[i])
|
|
<< "orientation covariance at " << i;
|
|
EXPECT_DOUBLE_EQ(out.angular_velocity_covariance[i], in.angular_velocity_covariance[i])
|
|
<< "angular velocity covariance at " << i;
|
|
EXPECT_DOUBLE_EQ(out.linear_acceleration_covariance[i], in.linear_acceleration_covariance[i])
|
|
<< "linear acceleration covariance at " << i;
|
|
}
|
|
}
|