void Link::setVariance(double rotVariance, double transVariance) { UASSERT(uIsFinite(rotVariance) && rotVariance>0); UASSERT(uIsFinite(transVariance) && transVariance>0); infMatrix_ = cv::Mat::eye(6,6,CV_64FC1); infMatrix_.at<double>(0,0) = 1.0/transVariance; infMatrix_.at<double>(1,1) = 1.0/transVariance; infMatrix_.at<double>(2,2) = 1.0/transVariance; infMatrix_.at<double>(3,3) = 1.0/rotVariance; infMatrix_.at<double>(4,4) = 1.0/rotVariance; infMatrix_.at<double>(5,5) = 1.0/rotVariance; }
// Metric constructor + 2d depth SensorData::SensorData(const cv::Mat & laserScan, int laserScanMaxPts, const cv::Mat & image, const cv::Mat & depthOrRightImage, float fx, float fyOrBaseline, float cx, float cy, const Transform & localTransform, const Transform & pose, float poseRotVariance, float poseTransVariance, int id, double stamp, const std::vector<unsigned char> & userData) : _image(image), _id(id), _stamp(stamp), _depthOrRightImage(depthOrRightImage), _laserScan(laserScan), _fx(fx), _fyOrBaseline(fyOrBaseline), _cx(cx), _cy(cy), _pose(pose), _localTransform(localTransform), _poseRotVariance(poseRotVariance), _poseTransVariance(poseTransVariance), _laserScanMaxPts(laserScanMaxPts), _userData(userData) { UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2); UASSERT(image.empty() || image.type() == CV_8UC1 || // Mono image.type() == CV_8UC3); // RGB UASSERT(depthOrRightImage.empty() || depthOrRightImage.type() == CV_32FC1 || // Depth in meter depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre depthOrRightImage.type() == CV_8U); // Right stereo image UASSERT(!_localTransform.isNull()); UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); }
void Link::setInfMatrix(const cv::Mat & infMatrix) { UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1); UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)"); infMatrix_ = infMatrix; }