void Run()
{
TC parent;
parent.position_valid = false;
// Create one with no heightmaps
ServerMapSector sector(&parent, v2s16(1,1));
UASSERT(sector.getBlockNoCreateNoEx(0) == 0);
UASSERT(sector.getBlockNoCreateNoEx(1) == 0);
MapBlock * bref = sector.createBlankBlock(-2);
UASSERT(sector.getBlockNoCreateNoEx(0) == 0);
UASSERT(sector.getBlockNoCreateNoEx(-2) == bref);
//TODO: Check for AlreadyExistsException
/*bool exception_thrown = false;
try{
sector.getBlock(0);
}
catch(InvalidPositionException &e){
exception_thrown = true;
}
UASSERT(exception_thrown);*/
}
void TestVoxelManipulator::testVoxelArea()
{
VoxelArea a(v3s16(-1,-1,-1), v3s16(1,1,1));
UASSERT(a.index(0,0,0) == 1*3*3 + 1*3 + 1);
UASSERT(a.index(-1,-1,-1) == 0);
VoxelArea c(v3s16(-2,-2,-2), v3s16(2,2,2));
// An area that is 1 bigger in x+ and z-
VoxelArea d(v3s16(-2,-2,-3), v3s16(3,2,2));
std::list<VoxelArea> aa;
d.diff(c, aa);
// Correct results
std::vector<VoxelArea> results;
results.push_back(VoxelArea(v3s16(-2,-2,-3), v3s16(3,2,-3)));
results.push_back(VoxelArea(v3s16(3,-2,-2), v3s16(3,2,2)));
UASSERT(aa.size() == results.size());
infostream<<"Result of diff:"<<std::endl;
for (std::list<VoxelArea>::const_iterator
it = aa.begin(); it != aa.end(); ++it) {
it->print(infostream);
infostream << std::endl;
std::vector<VoxelArea>::iterator j;
j = std::find(results.begin(), results.end(), *it);
UASSERT(j != results.end());
results.erase(j);
}
}
void TestMapSettingsManager::testMapMetaSaveLoad()
{
Settings conf;
std::string path = getTestTempDirectory()
+ DIR_DELIM + "foobar" + DIR_DELIM + "map_meta.txt";
// Create a set of mapgen params and save them to map meta
conf.set("seed", "12345");
conf.set("water_level", "5");
MapSettingsManager mgr1(&conf, path);
MapgenParams *params1 = mgr1.makeMapgenParams();
UASSERT(params1);
UASSERT(mgr1.saveMapMeta());
// Now try loading the map meta to mapgen params
conf.set("seed", "67890");
conf.set("water_level", "32");
MapSettingsManager mgr2(&conf, path);
UASSERT(mgr2.loadMapMeta());
MapgenParams *params2 = mgr2.makeMapgenParams();
UASSERT(params2);
// Check that both results are correct
UASSERTEQ(u64, params1->seed, 12345);
UASSERTEQ(s16, params1->water_level, 5);
UASSERTEQ(u64, params2->seed, 12345);
UASSERTEQ(s16, params2->water_level, 5);
}
void TestVoxelManipulator::testVoxelManipulator(INodeDefManager *nodedef)
{
VoxelManipulator v;
v.print(infostream, nodedef);
infostream << "*** Setting (-1,0,-1)=2 ***" << std::endl;
v.setNodeNoRef(v3s16(-1,0,-1), MapNode(t_CONTENT_GRASS));
v.print(infostream, nodedef);
UASSERT(v.getNode(v3s16(-1,0,-1)).getContent() == t_CONTENT_GRASS);
infostream << "*** Reading from inexistent (0,0,-1) ***" << std::endl;
EXCEPTION_CHECK(InvalidPositionException, v.getNode(v3s16(0,0,-1)));
v.print(infostream, nodedef);
infostream << "*** Adding area ***" << std::endl;
VoxelArea a(v3s16(-1,-1,-1), v3s16(1,1,1));
v.addArea(a);
v.print(infostream, nodedef);
UASSERT(v.getNode(v3s16(-1,0,-1)).getContent() == t_CONTENT_GRASS);
EXCEPTION_CHECK(InvalidPositionException, v.getNode(v3s16(0,1,1)));
}
void TestUtilities::testTrim()
{
UASSERT(trim("") == "");
UASSERT(trim("dirt_with_grass") == "dirt_with_grass");
UASSERT(trim("\n \t\r Foo bAR \r\n\t\t ") == "Foo bAR");
UASSERT(trim("\n \t\r \r\n\t\t ") == "");
}
AstNode* AstNode::addNext(AstNode* newp) {
// Add to m_nextp, returns this
UASSERT(newp,"Null item passed to addNext\n");
this->debugTreeChange("-addNextThs: ", __LINE__, false);
newp->debugTreeChange("-addNextNew: ", __LINE__, true);
if (this == NULL) {
return (newp);
} else {
// Find end of old list
AstNode* oldtailp = this;
if (oldtailp->m_nextp) {
if (oldtailp->m_headtailp) {
oldtailp = oldtailp->m_headtailp; // This=beginning of list, jump to end
UASSERT(!oldtailp->m_nextp, "Node had next, but headtail says it shouldn't");
} else {
// Though inefficent, we are occasionally passed a addNext in the middle of a list.
while (oldtailp->m_nextp != NULL) oldtailp = oldtailp->m_nextp;
}
}
// Link it in
oldtailp->m_nextp = newp;
newp->m_backp = oldtailp;
// New tail needs the head
AstNode* newtailp = newp->m_headtailp;
AstNode* headp = oldtailp->m_headtailp;
oldtailp->m_headtailp = NULL; // May be written again as new head
newp->m_headtailp = NULL; // May be written again as new tail
newtailp->m_headtailp = headp;
headp->m_headtailp = newtailp;
newp->editCountInc();
if (oldtailp->m_iterpp) *(oldtailp->m_iterpp) = newp; // Iterate on new item
}
this->debugTreeChange("-addNextOut:", __LINE__, true);
return this;
}
void TestSerialization::testDeSerializeLongString()
{
// Test deserialize
{
std::istringstream is(serializeLongString(teststring2), std::ios::binary);
UASSERT(deSerializeLongString(is) == teststring2);
UASSERT(!is.eof());
is.get();
UASSERT(is.eof());
}
// Test deserialize an incomplete length specifier
{
std::istringstream is(mkstr("\x53"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeLongString(is));
}
// Test deserialize a string with incomplete data
{
std::istringstream is(mkstr("\x00\x00\x00\x05 abc"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeLongString(is));
}
// Test deserialize a string with a length too large
{
std::istringstream is(mkstr("\xFF\xFF\xFF\xFF blah"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeLongString(is));
}
}
void TestSerialization::testDeSerializeWideString()
{
// Test deserialize
{
std::istringstream is(serializeWideString(teststring2_w), std::ios::binary);
UASSERT(deSerializeWideString(is) == teststring2_w);
UASSERT(!is.eof());
is.get();
UASSERT(is.eof());
}
// Test deserialize an incomplete length specifier
{
std::istringstream is(mkstr("\x53"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeWideString(is));
}
// Test deserialize a string with an incomplete character
{
std::istringstream is(mkstr("\x00\x07\0a\0b\0c\0d\0e\0f\0"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeWideString(is));
}
// Test deserialize a string with incomplete data
{
std::istringstream is(mkstr("\x00\x08\0a\0b\0c\0d\0e\0f"), std::ios::binary);
EXCEPTION_CHECK(SerializationError, deSerializeWideString(is));
}
}
// Mono constructor
SensorData::SensorData(
const cv::Mat & image,
const CameraModel & cameraModel,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_laserScanMaxRange(0.0f),
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(image.rows == 1)
{
UASSERT(image.type() == CV_8UC1); // Bytes
_imageCompressed = image;
}
else if(!image.empty())
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(int width, int height,
int bin_width, int bin_height,
double bin_depth,
int smoothing,
double max_depth) :
width_(width),
height_(height),
bin_width_(bin_width),
bin_height_(bin_height),
bin_depth_(bin_depth)
{
UASSERT(width_ % bin_width_ == 0);
UASSERT(height_ % bin_height_ == 0);
num_bins_x_ = width_ / bin_width_;
num_bins_y_ = height_ / bin_height_;
frustums_.resize(num_bins_y_);
for(size_t i = 0; i < frustums_.size(); ++i) {
frustums_[i].resize(num_bins_x_, NULL);
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(smoothing, bin_depth, max_depth);
}
training_samples_ = 0;
}
void TestVoxelAlgorithms::testClearLightAndCollectSources(INodeDefManager *ndef)
{
VoxelManipulator v;
for (u16 z = 0; z < 3; z++)
for (u16 y = 0; y < 3; y++)
for (u16 x = 0; x < 3; x++) {
v3s16 p(x,y,z);
v.setNode(p, MapNode(CONTENT_AIR));
}
VoxelArea a(v3s16(0,0,0), v3s16(2,2,2));
v.setNodeNoRef(v3s16(0,0,0), MapNode(t_CONTENT_STONE));
v.setNodeNoRef(v3s16(1,1,1), MapNode(t_CONTENT_TORCH));
{
MapNode n(CONTENT_AIR);
n.setLight(LIGHTBANK_DAY, 1, ndef);
v.setNode(v3s16(1,1,2), n);
}
{
std::set<v3s16> light_sources;
std::map<v3s16, u8> unlight_from;
voxalgo::clearLightAndCollectSources(v, a, LIGHTBANK_DAY,
ndef, light_sources, unlight_from);
//v.print(dstream, ndef, VOXELPRINT_LIGHT_DAY);
UASSERT(v.getNode(v3s16(0,1,1)).getLight(LIGHTBANK_DAY, ndef) == 0);
UASSERT(light_sources.find(v3s16(1,1,1)) != light_sources.end());
UASSERT(light_sources.size() == 1);
UASSERT(unlight_from.find(v3s16(1,1,2)) != unlight_from.end());
UASSERT(unlight_from.size() == 1);
}
}
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(uIsNan(z) || z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
z = zf;
}
}
}
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(uIsNan(z) || z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
z = zf*1000;
}
}
}
}
void TestUtilities::testMyround()
{
UASSERT(myround(4.6f) == 5);
UASSERT(myround(1.2f) == 1);
UASSERT(myround(-3.1f) == -3);
UASSERT(myround(-6.5f) == -7);
}
void TestUtilities::testStringAllowed()
{
UASSERT(string_allowed("hello", "abcdefghijklmno") == true);
UASSERT(string_allowed("123", "abcdefghijklmno") == false);
UASSERT(string_allowed_blacklist("hello", "123") == true);
UASSERT(string_allowed_blacklist("hello123", "123") == false);
}
virtual void runTest() {
V3Graph* gp = &m_graph;
// Verify we break edges at a good point
// A simple alg would make 3 breaks, below only requires b->i to break
V3GraphTestVertex* i = new V3GraphTestVarVertex(gp,"*INPUTS*");
V3GraphTestVertex* a = new V3GraphTestVarVertex(gp,"a");
V3GraphTestVertex* b = new V3GraphTestVarVertex(gp,"b");
V3GraphTestVertex* g1 = new V3GraphTestVarVertex(gp,"g1");
V3GraphTestVertex* g2 = new V3GraphTestVarVertex(gp,"g2");
V3GraphTestVertex* g3 = new V3GraphTestVarVertex(gp,"g3");
V3GraphTestVertex* q = new V3GraphTestVarVertex(gp,"q");
new V3GraphEdge(gp, i, a, 2, true);
new V3GraphEdge(gp, a, b, 2, true);
new V3GraphEdge(gp, b, g1, 2, true);
new V3GraphEdge(gp, b, g2, 2, true);
new V3GraphEdge(gp, b, g3, 2, true);
new V3GraphEdge(gp, g1, a, 2, true);
new V3GraphEdge(gp, g3, g2, 2, true);
new V3GraphEdge(gp, g2, g3, 2, true);
new V3GraphEdge(gp, g1, q, 2, true);
new V3GraphEdge(gp, g2, q, 2, true);
new V3GraphEdge(gp, g3, q, 2, true);
gp->stronglyConnected(&V3GraphEdge::followAlwaysTrue);
dump();
UASSERT(i->color()!=a->color() && a->color() != g2->color() && g2->color() != q->color(), "Separate colors not assigned");
UASSERT(a->color()==b->color() && a->color()==g1->color(), "Strongly connected nodes not colored together");
UASSERT(g2->color()==g3->color(), "Strongly connected nodes not colored together");
}
void TestUtilities::testStringTrim()
{
UASSERT(trim(" a") == "a");
UASSERT(trim(" a ") == "a");
UASSERT(trim("a ") == "a");
UASSERT(trim("") == "");
}
void TestServerShutdownState::testInit()
{
Server::ShutdownState ss;
UASSERT(!ss.is_requested);
UASSERT(!ss.should_reconnect);
UASSERT(ss.message.empty());
UASSERT(ss.m_timer == 0.0f);
}
void TestUtilities::testRemoveStringEnd()
{
const char *ends[] = {"abc", "c", "bc", "", NULL};
UASSERT(removeStringEnd("abc", ends) == "");
UASSERT(removeStringEnd("bc", ends) == "b");
UASSERT(removeStringEnd("12c", ends) == "12");
UASSERT(removeStringEnd("foo", ends) == "");
}
/**
* Only called once.
*/
static int setup(void)
{
int status = pthread_cond_init(&cond, NULL);
UASSERT(status == 0);
status = pthread_mutex_init(&mutex, NULL);
UASSERT(status == 0);
return 0;
}
const DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x) const
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
// Multi-cameras RGB-D constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_laserScanMaxRange(laserScanMaxRange),
_cameraModels(cameraModels)
{
if(rgb.rows == 1)
{
UASSERT(rgb.type() == CV_8UC1); // Bytes
_imageCompressed = rgb;
}
else if(!rgb.empty())
{
UASSERT(rgb.type() == CV_8UC1 || // Mono
rgb.type() == CV_8UC3); // RGB
_imageRaw = rgb;
}
if(depth.rows == 1)
{
UASSERT(depth.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = depth;
}
else if(!depth.empty())
{
UASSERT(depth.type() == CV_32FC1 || // Depth in meter
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2)
{
_laserScanRaw = laserScan;
}
else if(!laserScan.empty())
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Stereo constructor + 2d laser scan
SensorData::SensorData(
const cv::Mat & laserScan,
int laserScanMaxPts,
float laserScanMaxRange,
const cv::Mat & left,
const cv::Mat & right,
const StereoCameraModel & cameraModel,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_laserScanMaxRange(laserScanMaxRange),
_stereoCameraModel(cameraModel)
{
if(left.rows == 1)
{
UASSERT(left.type() == CV_8UC1); // Bytes
_imageCompressed = left;
}
else if(!left.empty())
{
UASSERT(left.type() == CV_8UC1 || // Mono
left.type() == CV_8UC3); // RGB
_imageRaw = left;
}
if(right.rows == 1)
{
UASSERT(right.type() == CV_8UC1); // Bytes
_depthOrRightCompressed = right;
}
else if(!right.empty())
{
UASSERT(right.type() == CV_8UC1); // Mono
_depthOrRightRaw = right;
}
if(laserScan.type() == CV_32FC2)
{
_laserScanRaw = laserScan;
}
else if(!laserScan.empty())
{
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Populate *outp with a minimal perfect matching of *this.
// *outp must be initially empty.
void perfectMatching(const std::vector<T_Key>& oddKeys,
TspGraphTmpl* outp) {
UASSERT(outp->empty(), "Output graph must start empty");
std::list<Vertex*> odds = keysToVertexList(oddKeys);
vl_unordered_set<Vertex*> unmatchedOdds;
typedef typename std::list<Vertex*>::iterator VertexListIt;
for (VertexListIt it = odds.begin(); it != odds.end(); ++it) {
outp->addVertex((*it)->key());
unmatchedOdds.insert(*it);
}
UASSERT(odds.size() % 2 == 0, "number of odd-order nodes should be even");
// TODO: The true Chrisofides algorithm calls for minimum-weight
// perfect matching. Instead, we have a simple greedy algorithm
// which might get close to the minimum, maybe, with luck?
//
// TODO: Revisit this. It's possible to compute the true minimum in
// N*N*log(N) time using variants of the Blossom algorithm.
// Implementing Blossom looks hard, maybe we can use an existing
// open source implementation -- for example the "LEMON" library
// which has a TSP solver.
// -----
// Reuse the comparator from Prim's routine. The logic is the same
// here. Note that the two V3GraphEdge's representing a single
// bidir edge will collide in the pendingEdges set here, but this
// is OK, we'll ignore the direction on the edge anyway.
EdgeCmp cmp;
typedef std::set<V3GraphEdge*, EdgeCmp&> PendingEdgeSet;
PendingEdgeSet pendingEdges(cmp);
for (VertexListIt it = odds.begin(); it != odds.end(); ++it) {
for (V3GraphEdge* edgep = (*it)->outBeginp();
edgep; edgep = edgep->outNextp()) {
pendingEdges.insert(edgep);
}
}
// Iterate over all edges, in order from low to high cost.
// For any edge whose ends are both odd-order vertices which
// haven't been matched yet, match them.
for (typename PendingEdgeSet::iterator it = pendingEdges.begin();
it != pendingEdges.end(); ++it) {
Vertex* fromp = castVertexp((*it)->fromp());
Vertex* top = castVertexp((*it)->top());
if ((unmatchedOdds.find(fromp) != unmatchedOdds.end())
&& (unmatchedOdds.find(top) != unmatchedOdds.end())) {
outp->addEdge(fromp->key(), top->key(), (*it)->weight());
unmatchedOdds.erase(fromp);
unmatchedOdds.erase(top);
}
}
UASSERT(unmatchedOdds.empty(), "Algorithm should have processed all vertices");
}
void TestServerShutdownState::testTrigger()
{
Server::ShutdownState ss;
ss.trigger(3.0f, "testtrigger", true);
UASSERT(!ss.is_requested);
UASSERT(ss.should_reconnect);
UASSERT(ss.message == "testtrigger");
UASSERT(ss.m_timer == 3.0f);
}
Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
UASSERT(!data.image().empty());
if(dynamic_cast<OdometryMono*>(this) == 0)
{
UASSERT(!data.depthOrRightImage().empty());
}
if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
{
UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
return Transform();
}
UTimer time;
Transform t = this->computeTransform(data, info);
if(info)
{
info->time = time.elapsed();
info->lost = t.isNull();
}
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
t = Transform(x,y,0, 0,0,yaw);
}
return _pose *= t; // updated
}
else if(_resetCurrentCount > 0)
{
UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
}
}
return Transform();
}
void CameraModel::initRectificationMap()
{
UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
UASSERT(R_.rows == 3 && R_.cols == 3);
UASSERT(P_.rows == 3 && P_.cols == 4);
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
void TestUtilities::testStringReplace()
{
std::string test_str;
test_str = "Hello there";
str_replace(test_str, "there", "world");
UASSERT(test_str == "Hello world");
test_str = "ThisAisAaAtest";
str_replace(test_str, 'A', ' ');
UASSERT(test_str == "This is a test");
}
void TestVoxelAlgorithms::testVoxelLineIterator(INodeDefManager *ndef)
{
// Test some lines
// Do not test lines that start or end on the border of
// two voxels as rounding errors can make the test fail!
std::vector<core::line3d<f32> > lines;
for (f32 x = -9.1; x < 9; x += 3.124) {
for (f32 y = -9.2; y < 9; y += 3.123) {
for (f32 z = -9.3; z < 9; z += 3.122) {
lines.push_back(core::line3d<f32>(-x, -y, -z, x, y, z));
}
}
}
lines.push_back(core::line3d<f32>(0, 0, 0, 0, 0, 0));
// Test every line
std::vector<core::line3d<f32> >::iterator it = lines.begin();
for (; it < lines.end(); it++) {
core::line3d<f32> l = *it;
// Initialize test
voxalgo::VoxelLineIterator iterator(l.start, l.getVector());
//Test the first voxel
v3s16 start_voxel = floatToInt(l.start, 1);
UASSERT(iterator.m_current_node_pos == start_voxel);
// Values for testing
v3s16 end_voxel = floatToInt(l.end, 1);
v3s16 voxel_vector = end_voxel - start_voxel;
int nodecount = abs(voxel_vector.X) + abs(voxel_vector.Y)
+ abs(voxel_vector.Z);
int actual_nodecount = 0;
v3s16 old_voxel = iterator.m_current_node_pos;
while (iterator.hasNext()) {
iterator.next();
actual_nodecount++;
v3s16 new_voxel = iterator.m_current_node_pos;
// This must be a neighbor of the old voxel
UASSERTEQ(f32, (new_voxel - old_voxel).getLengthSQ(), 1);
// The line must intersect with the voxel
v3f voxel_center = intToFloat(iterator.m_current_node_pos, 1);
aabb3f box(voxel_center - v3f(0.5, 0.5, 0.5),
voxel_center + v3f(0.5, 0.5, 0.5));
UASSERT(box.intersectsWithLine(l));
// Update old voxel
old_voxel = new_voxel;
}
// Test last node
UASSERT(iterator.m_current_node_pos == end_voxel);
// Test node count
UASSERTEQ(int, actual_nodecount, nodecount);
}
}
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
}
void TestSerialization::testSerializeString()
{
// Test blank string
UASSERT(serializeString("") == mkstr("\0\0"));
// Test basic string
UASSERT(serializeString("Hello world!") == mkstr("\0\14Hello world!"));
// Test character range
UASSERT(serializeString(teststring2) == mkstr("\1\0") + teststring2);
}