iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
tests/test-PointCloud2.C
/*! @file
@author Shane Grant
@copyright GNU Public License (GPL v3)
@section License
@verbatim
// ////////////////////////////////////////////////////////////////////////
// The iLab Neuromorphic Robotics Toolkit (NRT) //
// Copyright 2010-2012 by the University of Southern California (USC) //
// and the iLab at USC. //
// //
// iLab - University of Southern California //
// Hedco Neurociences Building, Room HNB-10 //
// Los Angeles, Ca 90089-2520 - USA //
// //
// See http://ilab.usc.edu for information about this project. //
// ////////////////////////////////////////////////////////////////////////
// This file is part of The iLab Neuromorphic Robotics Toolkit. //
// //
// The iLab Neuromorphic Robotics Toolkit is free software: you can //
// redistribute it and/or modify it under the terms of the GNU General //
// Public License as published by the Free Software Foundation, either //
// version 3 of the License, or (at your option) any later version. //
// //
// The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
// that it will be useful, but WITHOUT ANY WARRANTY; without even the //
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
// PURPOSE. See the GNU General Public License for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with The iLab Neuromorphic Robotics Toolkit. If not, see //
// <http://www.gnu.org/licenses/>. //
// ////////////////////////////////////////////////////////////////////////
@endverbatim */
#include <nrt/config.h>
#ifdef NRT_HAVE_CLOUD
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PointCloudTest
#include <boost/test/unit_test.hpp>
using namespace nrt;
namespace
{
bool isAligned( const void * __restrict__ pointer, size_t byte_count )
{
return (uintptr_t)pointer % byte_count == 0;
}
}
// ######################################################################
BOOST_AUTO_TEST_CASE( PointSSETest )
{
char unused = 2; // to help with alignment test
PointCloud2::Geometry aPoint( 3 );
BOOST_CHECK_EQUAL( aPoint.x(), 3 );
BOOST_CHECK_EQUAL( aPoint.y(), 3 );
BOOST_CHECK_EQUAL( aPoint.z(), 3 );
BOOST_CHECK_EQUAL( aPoint.w(), 0 );
aPoint.w() = 3;
BOOST_CHECK_EQUAL( aPoint.w(), 3 );
BOOST_CHECK_EQUAL( isAligned( &aPoint, 16 ), true );
auto eigenVec4 = aPoint.getVectorMap();
eigenVec4[3] = 3;
auto eigenVec3 = aPoint.getVector3Map();
eigenVec3[2] = 2;
BOOST_CHECK_EQUAL( aPoint.w(), 3 );
BOOST_CHECK_EQUAL( aPoint.z(), 2 );
assert(unused == 2);
aPoint.x() = 1;
aPoint.y() = 2;
aPoint.z() = 3;
aPoint.w() = 4;
BOOST_CHECK_EQUAL( aPoint.x(), 1 );
BOOST_CHECK_EQUAL( aPoint.y(), 2 );
BOOST_CHECK_EQUAL( aPoint.z(), 3 );
BOOST_CHECK_EQUAL( aPoint.w(), 4 );
}
// ######################################################################
BOOST_AUTO_TEST_CASE( PointCloud2DataTest )
{
BOOST_CHECK_EQUAL( isAligned( &data.geometry(), 16 ), true );
data.geometry().x() = 1;
data.geometry().y() = 2;
data.geometry().z() = 3;
data.geometry().w() = 4;
PointCloud2DataRef<> simpleDataRef(someGeo);
simpleDataRef = data;
BOOST_CHECK_EQUAL( someGeo.x(), 1 );
BOOST_CHECK_EQUAL( someGeo.y(), 2 );
BOOST_CHECK_EQUAL( someGeo.z(), 3 );
BOOST_CHECK_EQUAL( someGeo.w(), 4 );
BOOST_CHECK_EQUAL( isAligned( &dataWithFields.geometry(), 16 ), true );
BOOST_CHECK_EQUAL( dataWithFields.hasType<int>(), true );
BOOST_CHECK_EQUAL( dataWithFields.hasType<float>(), true );
BOOST_CHECK_EQUAL( dataWithFields.hasType<char>(), false );
dataWithFields = {{1, 2, 3, 4}, 5, 6.0f};
BOOST_CHECK_EQUAL( dataWithFields.geometry().x(), 1 );
BOOST_CHECK_EQUAL( dataWithFields.geometry().y(), 2 );
BOOST_CHECK_EQUAL( dataWithFields.geometry().z(), 3 );
BOOST_CHECK_EQUAL( dataWithFields.geometry().w(), 4 );
BOOST_CHECK_EQUAL( dataWithFields.get<int>(), 5 );
BOOST_CHECK_EQUAL( dataWithFields.get<float>(), 6.0f );
PointCloud2DataRef<int, float> dataRef( dataWithFields );
dataRef.geometry().x() = 33;
BOOST_CHECK_EQUAL( dataWithFields.geometry().x(), 33 );
BOOST_CHECK_EQUAL( dataWithFields.geometry().x(), dataRef.geometry().x() );
dataRef.get<float>() = 4.2f;
BOOST_CHECK_EQUAL( dataWithFields.get<float>(), 4.2f );
BOOST_CHECK_EQUAL( dataWithFields.get<float>(), dataRef.get<float>() );
PointCloud2ConstDataRef<int, float> dataConstRef( dataRef );
BOOST_CHECK_EQUAL( dataWithFields.get<float>(), dataConstRef.get<float>() );
dataWithFields.geometry().y() = 33;
BOOST_CHECK_EQUAL( dataConstRef.geometry().y(), 33 );
BOOST_CHECK_EQUAL( dataConstRef.geometry().y(), dataWithFields.geometry().y() );
PointCloud2Data<int, double> testInitializerList = {{3.0f}, 1, 2.0};
BOOST_CHECK_EQUAL( testInitializerList.geometry().y(), 3.0f );
BOOST_CHECK_EQUAL( testInitializerList.get<int>(), 1 );
BOOST_CHECK_EQUAL( testInitializerList.get<double>(), 2.0 );
testInitializerList = {{2.0f}, 3, 4.0};
BOOST_CHECK_EQUAL( testInitializerList.geometry().y(), 2.0f );
BOOST_CHECK_EQUAL( testInitializerList.get<int>(), 3 );
BOOST_CHECK_EQUAL( testInitializerList.get<double>(), 4.0 );
}
// ######################################################################
BOOST_AUTO_TEST_CASE( FieldPointCloudTests )
{
// Test adding fields
PointCloud2 cloud1;
cloud1.addField<>();
BOOST_CHECK_EQUAL( cloud1.hasSameFields( PointCloud2() ), true );
cloud1.addField<int, bool, char>();
BOOST_CHECK_EQUAL( cloud1.hasDenseField<int>(), true );
BOOST_CHECK_EQUAL( cloud1.hasDenseField<bool>(), true );
BOOST_CHECK_EQUAL( cloud1.hasDenseField<char>(), true );
BOOST_CHECK_EQUAL( cloud1.hasSparseField<int>(), false );
BOOST_CHECK_EQUAL( cloud1.hasDenseField<double>(), false );
BOOST_CHECK_EQUAL( cloud1.hasSparseField<double>(), true );
// Removing fields
cloud1.removeField<int, bool, char, double>();
BOOST_CHECK_EQUAL( cloud1.hasSameFields( PointCloud2() ), true );
// Static ctor
auto cloudFromStatic = PointCloud2::create<int, SparseField<bool>>();
BOOST_CHECK_EQUAL( cloudFromStatic.hasDenseField<int>(), true );
BOOST_CHECK_EQUAL( cloudFromStatic.hasSparseField<bool>(), true );
}
// ######################################################################
BOOST_AUTO_TEST_CASE( AssignmentAndMovingTests )
{
PointCloud2 cloud1;
cloud1.insert( {3} );
cloud1.insert( {4} );
cloud1.insert( {5} );
PointCloud2 cloud2( cloud1 );
BOOST_CHECK_EQUAL( cloud2[0].x(), cloud1[0].x() );
BOOST_CHECK_EQUAL( cloud2[1].y(), cloud1[1].y() );
BOOST_CHECK_EQUAL( cloud2[2].z(), cloud1[2].z() );
cloud1 = cloud2;
BOOST_CHECK_EQUAL( cloud2[0].x(), cloud1[0].x() );
BOOST_CHECK_EQUAL( cloud2[1].y(), cloud1[1].y() );
BOOST_CHECK_EQUAL( cloud2[2].z(), cloud1[2].z() );
PointCloud2 cloud3 = PointCloud2::create<int>(1);
cloud3.get<int>( 0 ) = data;
BOOST_CHECK_EQUAL( cloud3[0].x(), 1 );
BOOST_CHECK_EQUAL( cloud3.at<int>(0).get<int>(), 5 );
cloud3.get<int>( 0 ) = PointCloud2Data<int>({2}, 4);
BOOST_CHECK_EQUAL( cloud3[0].x(), 2 );
BOOST_CHECK_EQUAL( cloud3.at<int>(0).get<int>(), 4 );
PointCloud2 cloud4( std::move( cloud3 ) ); // cloud3 no longer valid, attempting to access will likely crash
BOOST_CHECK_EQUAL( cloud4[0].x(), 2 );
BOOST_CHECK_EQUAL( cloud4.at<int>(0).get<int>(), 4 );
}
struct dummyStruct
{
int x;
};
// ######################################################################
BOOST_AUTO_TEST_CASE( InsertingStuffTests )
{
PointCloud2 cloudInsert = PointCloud2::create<int, bool, SparseField<double>>();
// Insert valid stuff
cloudInsert.insert<int, bool, double>( {{3.0f}, 1, true, 3.2} );
auto cloudInsertRef1 = cloudInsert.at<int, bool, double>( 0 );
BOOST_CHECK_EQUAL( cloudInsertRef1.geometry().x(), 3.0f );
BOOST_CHECK_EQUAL( cloudInsertRef1.get<int>(), 1 );
BOOST_CHECK_EQUAL( cloudInsertRef1.get<bool>(), true );
BOOST_CHECK_EQUAL( cloudInsertRef1.get<double>(), 3.2 );
// Insert a non fully dense point
cloudInsert.insert( {2.9f} );
auto cloudInsertRef2 = cloudInsert.at<int, bool>( 1 );
BOOST_CHECK_EQUAL( cloudInsertRef2.geometry().x(), 2.9f );
BOOST_CHECK_EQUAL( cloudInsertRef2.get<int>(), 0 );
BOOST_CHECK_EQUAL( cloudInsertRef2.get<bool>(), false );
cloudInsert.addField<dummyStruct>();
cloudInsert.get<dummyStruct>( 0 ).get<dummyStruct>().x = 3;
BOOST_CHECK_EQUAL( cloudInsert.at<dummyStruct>( 0 ).get<dummyStruct>().x, 3 );
bool tryResult = false;
try
{
// Should throw as there is no data here for sparse
cloudInsert.at<double>( 1 );
}
catch( std::exception const & e )
{
tryResult = true;
}
BOOST_CHECK_EQUAL( tryResult, true );
// add sparse data and check it
cloudInsert.insertSparse<double>( 1, {29.2} );
BOOST_CHECK_EQUAL( cloudInsert.at<double>( 1 ).get<double>(), 29.2 );
// delete all data from a field
try
{
cloudInsert.at( 1 );
tryResult = false;
}
catch( std::exception const & e )
{
tryResult = true;
}
BOOST_CHECK_EQUAL( cloudInsert.at<double>( 1 ).get<double>(), 29.2 );
cloudInsert.resize( 0 );
for( size_t i = 0; i < 100; ++i )
cloudInsert.insert({});
BOOST_CHECK_EQUAL( cloudInsert.size(), 100 );
BOOST_CHECK_EQUAL( cloudInsert.size<double>(), 0 );
cloudInsert.resize( 0 );
BOOST_CHECK_EQUAL( cloudInsert.empty(), true );
cloudInsert.insert( {1, 2, 3, 4} );
BOOST_CHECK_EQUAL( cloudInsert[0], cloudInsert.at( 0 ).geometry() );
BOOST_CHECK_EQUAL( PointCloud2::Geometry(1, 2, 3, 4), cloudInsert.get( 0 ).geometry() );
}
//// bug in g++, the following should work but does not:
//template <bool Whatever>
//void testFunction( nrt::PointCloud2 const & cloud )
//{
// for( auto i : cloud.range<int>() )
// i.get<int>() = 0;
//}
// ######################################################################
BOOST_AUTO_TEST_CASE( IteratorSchmiterator )
{
PointCloud2 cloud = PointCloud2::create<int, SparseField<double>>();
// Populate with junk geometry
for( size_t i = 0; i < 1000; ++i )
cloud.insert<int>( {{(float)i}, (int)i} );
int index = 0;
for( auto i = cloud.geometry_begin(), end = cloud.geometry_end(); i != end; ++i, ++index )
BOOST_CHECK_EQUAL( *i, PointCloud2::Geometry((float)index) );
index = 0;
for( auto i = cloud.geometry_const_begin(), end = cloud.geometry_const_end(); i != end; ++i, ++index )
BOOST_CHECK_EQUAL( *i, PointCloud2::Geometry((float)index) );
index = 0;
for( auto i = cloud.begin(), end = cloud.end(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i.index(), index );
}
index = 0;
for( auto i = cloud.const_begin(), end = cloud.const_end(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i.index(), index );
}
index = 0;
for( auto i : cloud.range() )
{
BOOST_CHECK_EQUAL( i.geometry(), PointCloud2::Geometry((float)index) );
index++;
}
// iterating over a dense field
index = 0;
for( auto i = cloud.begin<int>(), end = cloud.end<int>(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i->get<int>(), index );
BOOST_CHECK_EQUAL( i.index(), index );
}
index = 0;
for( auto i = cloud.const_begin<int>(), end = cloud.const_end<int>(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i->get<int>(), index );
BOOST_CHECK_EQUAL( i.index(), index );
}
index = 0;
for( auto i : cloud.range<int>() )
{
BOOST_CHECK_EQUAL( i.geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i.get<int>(), index );
index++;
}
index = 0;
for( auto i : cloud.const_range<int>() )
{
BOOST_CHECK_EQUAL( i.geometry(), PointCloud2::Geometry((float)index) );
BOOST_CHECK_EQUAL( i.get<int>(), index );
index++;
}
// error if field not there
bool ok = false;
try
{
cloud.begin<char>();
}
catch( ... )
{
ok = true;
}
BOOST_CHECK_EQUAL( ok, true );
// iterate over sparse data
cloud.insertSparse<double>( 5, {3.2} );
cloud.insertSparse<double>( 17, {4.2} );
cloud.insertSparse<double>( 33, {5.2} );
cloud.insertSparse<double>( 944, {6.2} );
std::array<int, 4> indicesSparse = {{5, 17, 33, 944}};
std::array<double, 4> dataSparse = {{3.2, 4.2, 5.2, 6.2}};
index = 0;
for( auto i = cloud.begin<int, double>(), end = cloud.end<int, double>(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i->get<int>(), indicesSparse[index] );
BOOST_CHECK_EQUAL( i.index(), indicesSparse[index] );
}
index = 0;
for( auto i = cloud.const_begin<int, double>(), end = cloud.const_end<int, double>(); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i->get<int>(), indicesSparse[index] );
BOOST_CHECK_EQUAL( i.index(), indicesSparse[index] );
}
index = 0;
for( auto i : cloud.range<int, double>() )
{
BOOST_CHECK_EQUAL( i.get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i.get<int>(), indicesSparse[index] );
++index;
}
index = 0;
for( auto i : cloud.const_range<int, double>() )
{
BOOST_CHECK_EQUAL( i.get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i.get<int>(), indicesSparse[index] );
++index;
}
Indices subset;
subset.push_back( 17 );
subset.push_back( 33 );
index = 0;
for( auto i = cloud.subset_begin( subset ), end = cloud.subset_end( subset ); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry( subset[index] ) );
BOOST_CHECK_EQUAL( i.index(), subset[index] );
}
index = 0;
for( auto i = cloud.subset_const_begin( subset ), end = cloud.subset_const_end( subset ); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->geometry(), PointCloud2::Geometry( subset[index] ) );
BOOST_CHECK_EQUAL( i.index(), subset[index] );
}
index = 0;
for( auto i : cloud.subset_range( subset ) )
{
BOOST_CHECK_EQUAL( i.geometry(), PointCloud2::Geometry( subset[index] ) );
++index;
}
index = 0;
for( auto i : cloud.subset_const_range( subset ) )
{
BOOST_CHECK_EQUAL( i.geometry(), PointCloud2::Geometry( subset[index] ) );
++index;
}
index = 1;
for( auto i = cloud.subset_begin<int, double>( subset ), end = cloud.subset_end<int, double>( subset ); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i->get<int>(), indicesSparse[index] );
BOOST_CHECK_EQUAL( i.index(), indicesSparse[index] );
}
index = 1;
for( auto i = cloud.subset_const_begin<int, double>( subset ), end = cloud.subset_const_end<int, double>( subset ); i != end; ++i, ++index )
{
BOOST_CHECK_EQUAL( i->get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i->get<int>(), indicesSparse[index] );
BOOST_CHECK_EQUAL( i.index(), indicesSparse[index] );
}
index = 1;
for( auto i : cloud.subset_range<int, double>( subset ) )
{
BOOST_CHECK_EQUAL( i.get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i.get<int>(), indicesSparse[index] );
++index;
}
index = 1;
for( auto i : cloud.subset_const_range<int, double>( subset ) )
{
BOOST_CHECK_EQUAL( i.get<double>(), dataSparse[index] );
BOOST_CHECK_EQUAL( i.get<int>(), indicesSparse[index] );
++index;
}
}
// ######################################################################
BOOST_AUTO_TEST_CASE( ReferenceCountingStuff )
{
PointCloud2 cloud;
cloud.insert({1, 2, 3, 4});
PointCloud2 cloud2 = cloud;
BOOST_CHECK_EQUAL( cloud == cloud2, true );
// const access
cloud2.at(0);
BOOST_CHECK_EQUAL( cloud == cloud2, true );
// non const access triggers deep copy
cloud2.get(0);
BOOST_CHECK_EQUAL( cloud != cloud2, true );
cloud2.get(0) = PointCloud2Data<>{{2, 3, 4, 5}};
BOOST_CHECK_NE( cloud[0].x(), cloud2[0].x() );
BOOST_CHECK_EQUAL( cloud2[0].z(), 4 );
BOOST_CHECK_EQUAL( cloud[0].z(), 3 );
PointCloud2 cloud3 = cloud2;
BOOST_CHECK_EQUAL( cloud3 == cloud2, true );
cloud3.deepCopy();
BOOST_CHECK_EQUAL( cloud3 != cloud2, true );
}
// ######################################################################
BOOST_AUTO_TEST_CASE( AppendageTest )
{
PointCloud2 cloud1(10);
float i = 0.0f;
for( auto & p : cloud1.geometry_range() )
{
p = {i};
i += 1.0f;
}
PointCloud2 cloud2(11);
for( auto & p : cloud2.geometry_range() )
{
p = {i};
i += 1.0f;
}
cloud1.append( cloud2 );
i = 0.0f;
for( auto const & p : cloud1.geometry_const_range() )
{
BOOST_CHECK_EQUAL( p, PointCloud2::Geometry(i) );
i += 1.0f;
}
cloud2.clear();
cloud2.resize(10);
cloud2.addField<int, SparseField<bool>>();
i = 0.0f;
for( auto p : cloud2.range<int>() )
{
p.geometry() = {i};
p.get<int>() = i;
i += 1.0f;
}
cloud2.insertSparse<bool>( 0, {false} );
cloud2.insertSparse<bool>( 6, {true} );
cloud2 = cloud2.append(cloud2);
i = 0.0f;
for( auto p : cloud2.const_range<int>() )
{
BOOST_CHECK_EQUAL( p.geometry(), PointCloud2::Geometry{i} );
BOOST_CHECK_EQUAL( p.get<int>(), int(i) );
i += 1.0f;
if( i > 9.9f )
i = 0.0f;
}
std::array<size_t, 4> goodIndices = {{0, 6, 10, 16}};
size_t index = 0;
for( auto i = cloud2.const_begin<bool>(), end = cloud2.const_end<bool>(); i != end; ++i )
BOOST_CHECK_EQUAL( i.index(), goodIndices[index++] );
}
// ######################################################################
BOOST_AUTO_TEST_CASE( sadf )
{
cloud.insert( geo );
cloud.insert( {1, 2, 3} );
cloud.insert( nrt::PointCloud2::Geometry( 4, 5, 6 ) );
}
#else
int main( int argc, char ** argv )
{
return 1;
}
#endif // NRT_HAVE_CLOUD