5 #include <gtest/gtest.h> 16 unsigned img_height { 480 };
17 unsigned img_width { 640 };
18 unsigned scan_height { 420 };
31 info_msg.reset(
new sensor_msgs::CameraInfo);
32 info_msg->header.frame_id =
"depth_frame";
33 info_msg->header.seq = 100;
34 info_msg->height = img_height;
35 info_msg->width = img_width;
36 info_msg->distortion_model =
"plumb_bob";
37 info_msg->D.resize(5);
50 info_msg->P[10] = 1.0;
55 depth_msg.reset(
new sensor_msgs::Image);
56 depth_msg->header.frame_id =
"depth_frame";
57 depth_msg->header.seq = 100;
58 depth_msg->header.stamp.fromSec(10.0);
59 depth_msg->height = img_height;
60 depth_msg->width = img_width;
61 depth_msg->is_bigendian =
false;
62 depth_msg->step = depth_msg->width *
sizeof(T);
64 if (
typeid(T) ==
typeid(uint16_t)) {
67 else if (
typeid(T) ==
typeid(float)) {
71 depth_msg->data.resize(depth_msg->width * depth_msg->height *
sizeof(T));
72 T* depth_row =
reinterpret_cast<T*
>(&depth_msg->data[0]);
73 for (
size_t i = 0; i < depth_msg->width * depth_msg->height; ++i) {
81 setDefaultDepthMsg<uint16_t>(1);
82 estimator.estimateParams(depth_msg, info_msg);
84 setDefaultDepthMsg<float>(1);
85 estimator.estimateParams(depth_msg, info_msg);
90 setDefaultDepthMsg<uint16_t>(1);
92 EXPECT_ANY_THROW(estimator.estimateParams(depth_msg, info_msg));
95 int main(
int argc,
char **argv)
97 testing::InitGoogleTest(&argc, argv);
98 return RUN_ALL_TESTS();
void setRansacMaxIter(const unsigned u)
sensor_msgs::CameraInfoPtr info_msg
const std::string TYPE_32FC1
TEST_F(DepthSensorPoseTest, encodingSupport)
const std::string TYPE_16UC1
sensor_msgs::ImagePtr depth_msg
void setDepthImgStepCol(const int step)
setDepthImgStepCol
void setDefaultDepthMsg(T value)
void setDepthImgStepRow(const int step)
setDepthImgStepRow
int main(int argc, char **argv)
void setGroundMaxPoints(const unsigned u)
DepthSensorPoseTestable estimator