35 using namespace Eigen;
41 const std::map<int, rtabmap::SensorData> & sequence,
42 const std::map<int, rtabmap::Transform> & trajectory,
43 const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
45 double coneStdevThresh)
51 const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
58 for(std::map<int, rtabmap::Transform>::const_iterator iter = trajectory.begin(); iter != trajectory.end(); ++iter)
60 size_t idx = iter->first;
61 std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
62 if(ster!=sequence.end())
65 ster->second.uncompressDataConst(0, &depthImage);
69 mapDepth = projector.
estimateMapDepth(map, iter->second.inverse(), depthImage, coneRadius, coneStdevThresh);
70 counts = model.
accumulate(mapDepth, depthImage);
73 UINFO(
"counts=%d", (
int)counts);
cv::Mat estimateMapDepth(const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, const rtabmap::Transform &transform, const cv::Mat &measurement, double coneRadius=0.02, double coneStdevThresh=0.03) const
DiscreteDepthDistortionModel RTABMAP_EXP calibrate(const std::map< int, rtabmap::SensorData > &sequence, const std::map< int, rtabmap::Transform > &trajectory, const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, double coneRadius=0.02, double coneStdevThresh=0.03)
size_t accumulate(const cv::Mat &ground_truth, const cv::Mat &measurement)
ULogger class and convenient macros.