bool Mapper::reset(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
	waitForMapBuildingCompleted();
	
	// note: no need for locking as we do ros::spin(), to update if we go for multi-threading
	publishLock.lock();
	TOdomToMap = PM::TransformationParameters::Identity(4,4);
	publishLock.unlock();

	icp.clearMap();
	
	return true;
}
예제 #2
0
TEST(icpTest, icpSequenceTest)
{
	DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
	DP pts1 = DP::load(dataPath + "cloud.00001.vtk");
	DP pts2 = DP::load(dataPath + "cloud.00002.vtk");
	
	PM::TransformationParameters Ticp   = PM::Matrix::Identity(4,4);

	PM::ICPSequence icpSequence;

	std::ifstream ifs((dataPath + "default.yaml").c_str());
	icpSequence.loadFromYaml(ifs);

	EXPECT_FALSE(icpSequence.hasMap());

	DP map = icpSequence.getInternalMap();
	EXPECT_EQ(map.getNbPoints(), 0u);
	EXPECT_EQ(map.getHomogeneousDim(), 0u);
	
	map = icpSequence.getMap();
	EXPECT_EQ(map.getNbPoints(), 0u);
	EXPECT_EQ(map.getHomogeneousDim(), 0u);

	icpSequence.setMap(pts0);
	map = icpSequence.getInternalMap();
	EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
	EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());

	Ticp = icpSequence(pts1);
	map = icpSequence.getMap();
	EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
	EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());
	
	Ticp = icpSequence(pts2);
	map = icpSequence.getMap();
	EXPECT_EQ(map.getNbPoints(), pts0.getNbPoints());
	EXPECT_EQ(map.getHomogeneousDim(), pts0.getHomogeneousDim());

	icpSequence.clearMap();
	map = icpSequence.getInternalMap();
	EXPECT_EQ(map.getNbPoints(), 0u);
	EXPECT_EQ(map.getHomogeneousDim(), 0u);



}