laser/updater.cpp File Reference

#include "ed/laser/updater.h"
#include "ed_sensor_integration/association_matrix.h"
#include <ed/convex_hull_calc.h>
#include <ed/entity.h>
#include <ed/update_request.h>
#include <ed/io/json_writer.h>
#include <ed/world_model.h>
#include <geolib/Shape.h>
#include <ros/console.h>
#include <tue/profiling/timer.h>
#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>

Include dependency graph for laser/updater.cpp:

Go to the source code of this file.

Macros
#define	MIN_N_POINTS_FITTING   3

Functions
bool	associateSegmentsWithEntities (std::vector< EntityUpdate > &clusters, const std::vector< ed::EntityConstPtr > &entities, double current_time, ed_sensor_integration::Assignment &assig)
	Associate segments with etities in the world model. More...
std::vector< ed::EntityConstPtr >	findNearbyEntities (std::vector< EntityUpdate > &clusters, const ed::WorldModel &world)
	findNearbyEntities create a list of entities that are close to detected clusters More...
geo::Pose3D	fitEntity (const ed::Entity &e, const geo::Pose3D &sensor_pose, const geo::LaserRangeFinder &lrf, const std::vector< double > &sensor_ranges, const std::vector< double > &model_ranges, double x_window, double x_step, double y_window, double y_step, double yaw_min, double yaw_plus, double yaw_step, std::map< ed::UUID, geo::Pose3D > &pose_cache)
	estimate the pose of an entity that best describes the sensor data. More...
double	getFittingError (const ed::Entity &e, const geo::LaserRangeFinder &lrf, const geo::Pose3D &rel_pose, const std::vector< double > &sensor_ranges, const std::vector< double > &model_ranges, int &num_model_points)
	Calculate an error based on the quality of a fit. More...
geo::Pose3D	getPoseFromCache (const ed::Entity &e, std::map< ed::UUID, geo::Pose3D > &pose_cache)

Macro Definition Documentation

MIN_N_POINTS_FITTING

#define MIN_N_POINTS_FITTING   3

Definition at line 20 of file laser/updater.cpp.

Function Documentation

associateSegmentsWithEntities()

bool associateSegmentsWithEntities	(	std::vector< EntityUpdate > &	clusters,
		const std::vector< ed::EntityConstPtr > &	entities,
		double	current_time,
		ed_sensor_integration::Assignment &	assig
	)

Associate segments with etities in the world model.

Parameters

[in]	clusters	Measured clusters
[in]	entities	Entities that may be associated with the clusters
[in]	current_time	Current time to compare against the last measurement of an entity
[out]	assig	Assignment matrix between clusters and entities.

Returns

Whether or not the assignment was successful

Definition at line 217 of file laser/updater.cpp.

findNearbyEntities()

std::vector<ed::EntityConstPtr> findNearbyEntities	(	std::vector< EntityUpdate > &	clusters,
		const ed::WorldModel &	world
	)

findNearbyEntities create a list of entities that are close to detected clusters

Parameters

clusters	detected clusters
world	worldmodel to get the entities from

Returns

entities that are near the clusters.

Definition at line 161 of file laser/updater.cpp.

fitEntity()

geo::Pose3D fitEntity	(	const ed::Entity &	e,
		const geo::Pose3D &	sensor_pose,
		const geo::LaserRangeFinder &	lrf,
		const std::vector< double > &	sensor_ranges,
		const std::vector< double > &	model_ranges,
		double	x_window,
		double	x_step,
		double	y_window,
		double	y_step,
		double	yaw_min,
		double	yaw_plus,
		double	yaw_step,
		std::map< ed::UUID, geo::Pose3D > &	pose_cache
	)

estimate the pose of an entity that best describes the sensor data.

Parameters

e	Entity to be fitted
sensor_pose	Pose of the sensor in world coordinates
lrf	Model of the laser range finder
sensor_ranges	Distances measured by the lrf
model_ranges	Predicted measurement distances using the model sans e
x_window	Window size to sample candidate poses
x_step	Step size to sample candidate poses
y_window	Window size to sample candidate poses
y_step	Step size to sample candidate poses
yaw_min,yaw_plus	window size to sample candidate poses
yaw_step	Step size to sample candidate poses
pose_cache	Cache of entity poses

Returns

Estimated pose of the entity

Definition at line 115 of file laser/updater.cpp.

getFittingError()

double getFittingError	(	const ed::Entity &	e,
		const geo::LaserRangeFinder &	lrf,
		const geo::Pose3D &	rel_pose,
		const std::vector< double > &	sensor_ranges,
		const std::vector< double > &	model_ranges,
		int &	num_model_points
	)

Calculate an error based on the quality of a fit.

Parameters

	e	Entity being fitted
	lrf	Laser Range Finder model
	rel_pose	Candidate pose of the entity relative to the laser range finder
	sensor_ranges	Distances measured by the lrf
	model_ranges	Predicted measurement distances using the model sans e
[out]	num_model_points	Number of points used for

Returns

double error

Definition at line 33 of file laser/updater.cpp.

getPoseFromCache()

geo::Pose3D getPoseFromCache	(	const ed::Entity &	e,
		std::map< ed::UUID, geo::Pose3D > &	pose_cache
	)

Definition at line 83 of file laser/updater.cpp.