i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added some asserts to chaeck inf variances

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-02 11:32:58 -04:00
parent 8458b25292
commit 653248bde8
2 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/include/rtabmap/core/Link.h
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Transform.h> #include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
namespace rtabmap { namespace rtabmap {
@@ -53,7 +54,7 @@ public:
rotVariance_(rotVariance), rotVariance_(rotVariance),
transVariance_(transVariance) transVariance_(transVariance)
{ {
UASSERT_MSG(rotVariance_ > 0 && transVariance_ > 0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
} }
bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;} bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -69,7 +70,11 @@ public:
void setTo(int to) {to_ = to;} void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;} void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;} void setType(Type type) {type_ = type;}
void setVariance(float rotVariance, float transVariance) {rotVariance_ = rotVariance; transVariance_ = transVariance;} void setVariance(float rotVariance, float transVariance) {
UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
rotVariance_ = rotVariance;
transVariance_ = transVariance;
}
private: private:
int from_; int from_;

5
corelib/src/SensorData.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/SensorData.h" #include "rtabmap/core/SensorData.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UMath.h>
namespace rtabmap namespace rtabmap
{ {
@@ -103,7 +104,7 @@ SensorData::SensorData(const cv::Mat & image,
depthOrRightImage.type() == CV_8U); // Right stereo image depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f); UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
UASSERT(!_localTransform.isNull()); UASSERT(!_localTransform.isNull());
UASSERT_MSG(_poseRotVariance>0 && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
} }
// Metric constructor + 2d depth // Metric constructor + 2d depth
@@ -144,7 +145,7 @@ SensorData::SensorData(const cv::Mat & laserScan,
depthOrRightImage.type() == CV_8U); // Right stereo image depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f); UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
UASSERT(!_localTransform.isNull()); UASSERT(!_localTransform.isNull());
UASSERT_MSG(_poseRotVariance>0 && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)"); UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
} }
bool SensorData::empty() const bool SensorData::empty() const