The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. More...

#include <StereoOdometer.hpp>

Inheritance diagram for viso2::StereoOdometer:

Public Member Functions
	StereoOdometer (std::string const &name="viso2::StereoOdometer")

	StereoOdometer (std::string const &name, RTT::ExecutionEngine *engine)

	~StereoOdometer ()

bool	configureHook ()
	The following lines are template definitions for the various state machine. More...

bool	startHook ()

void	updateHook ()

void	errorHook ()

void	stopHook ()

void	cleanupHook ()

viso2::Viso2Info	computeStereoOdometer (const base::Time &ts, const Eigen::Affine3d &tf)
	Computes one step of stereo visual odometry. More...

void	drawMatches (const base::samples::frame::Frame &image1, const base::samples::frame::Frame &image2, const std::vector< Matcher::p_match > &matches, const std::vector< int32_t > &inlier_indices, base::samples::frame::Frame &imageOutput)

void	createDistanceImage (const base::samples::frame::Frame &image1, const base::samples::frame::Frame &image2, const std::vector< Matcher::p_match > &matches, const VisualOdometryStereo::parameters &viso2param, base::samples::DistanceImage &distImage)

void	createPointCloud (const Eigen::Affine3d &tf, const base::samples::frame::Frame &image1, const std::vector< Matcher::p_match > &matches, const std::vector< int32_t > &inlier_indices, const Eigen::Matrix4d &Q, const Eigen::Affine3d &deltaPose, boost::unordered_map< int32_t, int32_t > &hashIdx, boost::circular_buffer< std::map< int32_t, HashPoint, std::less< int32_t >, Eigen::aligned_allocator< std::pair< const int32_t, HashPoint > > > > &hashPointcloud)

base::Vector3d	computeFeaturesJacobian (const Eigen::Affine3d &deltaPose, const base::Vector3d &point)

void	postProcessPointCloud (boost::unordered_map< int32_t, int32_t > &hashIdx, boost::circular_buffer< std::map< int32_t, HashPoint, std::less< int32_t >, Eigen::aligned_allocator< std::pair< const int32_t, HashPoint > > > > &hashPointcloud, base::samples::Pointcloud &pointcloud, std::vector< base::Matrix3d > &pointsVar, std::vector< unsigned int > &pointsIdx, base::MatrixXd &deltaJacobCurr, base::MatrixXd &deltaJacobPrev)

Protected Member Functions
virtual void	left_frameTransformerCallback (const base::Time &ts, const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &left_frame_sample)

virtual void	right_frameTransformerCallback (const base::Time &ts, const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &right_frame_sample)

Protected Attributes
VisualOdometryStereo::parameters	viso2param

frame_helper::StereoCalibration	cameracalib

frame_helper::StereoCalibrationCv	cameracalibCv

boost::shared_ptr< VisualOdometryStereo >	viso

boost::circular_buffer< base::samples::frame::FramePair >	imagePair

frame_helper::FrameHelper	frameHelperLeft

frame_helper::FrameHelper	frameHelperRight

Eigen::Matrix4d	Q

::base::samples::frame::Frame	leftColorImage

::base::Matrix2d	pxleftVar

::base::Matrix2d	pxrightVar

boost::unordered_map< int32_t, int32_t >	hashIdx

boost::circular_buffer< std::map< int32_t, HashPoint, std::less< int32_t >, Eigen::aligned_allocator< std::pair< const int32_t, HashPoint > > > >	hashPointcloud

Eigen::Affine3d	pose

base::samples::RigidBodyState	poseOut

RTT::extras::ReadOnlyPointer< base::samples::frame::Frame >	frame_out

Static Protected Attributes
static const int	DEFAULT_CIRCULAR_BUFFER_SIZE = 2

Friends
class	StereoOdometerBase

Detailed Description

The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow.

The name of a TaskContext is primarily defined via:

deployment 'deployment_name'
    task('custom_task_name','viso2::StereoOdometer')
end

It can be dynamically adapted when the deployment is called with a prefix argument.

Constructor & Destructor Documentation

StereoOdometer::StereoOdometer ( std::string const & name = "viso2::StereoOdometer" )

TaskContext constructor for StereoOdometer

Parameters

name	Name of the task. This name needs to be unique to make it identifiable via nameservices.
initial_state	The initial TaskState of the TaskContext. Default is Stopped state.

StereoOdometer::StereoOdometer	(	std::string const &	name,
		RTT::ExecutionEngine *	engine
	)

TaskContext constructor for StereoOdometer

Parameters

name	Name of the task. This name needs to be unique to make it identifiable for nameservices.
engine	The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task.

StereoOdometer::~StereoOdometer ( )

Default deconstructor of StereoOdometer

Member Function Documentation

void StereoOdometer::cleanupHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to PreOperational, requiring the call to configureHook() before calling start() again.

base::Vector3d StereoOdometer::computeFeaturesJacobian	(	const Eigen::Affine3d &	deltaPose,
		const base::Vector3d &	point
	)

Experimental

viso2::Viso2Info StereoOdometer::computeStereoOdometer	(	const base::Time &	ts,
		const Eigen::Affine3d &	tf
	)

Computes one step of stereo visual odometry.

Time

Delta pose

Get the images to plain pointers

Compute visual odometry

On success get the delta transform and update the pose

Map the viso2 data to an Eigen matrix

Transform the deltaPose in the body frame

On success, update current pose

Output some statistics

Store in RigidBodyState to port out the pose

In Euler angles

Fill the delta pose out

Port out the delta pose

Port out the accumulated pose

Draw matches in the images using the FrameHelper which internally uses openCV

Create the point cloud from the current pair

Re-arrange the point cloud and compute the uncertainty

Port out the information

bool StereoOdometer::configureHook ( )

The following lines are template definitions for the various state machine.

This hook is called by Orocos when the state machine transitions from PreOperational to Stopped. If it returns false, then the component will stay in PreOperational. Otherwise, it goes into Stopped.

It is meaningful only if the #needs_configuration has been specified in the task context definition with (for example):

task_context "TaskName" do
  needs_configuration
  ...
end

Read the camera calibration parameters

Read the dedicated viso2 parameter configuration values

Set the calibration parameters in the viso2 type

Re-projection Q matrix

Image plane uncertainty(variance) in pixel

Initialize variables

Frame Helper

Initial pose initialization

Rigid body state output initialization

Stereo working pair

Hash Table of indexes

void StereoOdometer::createDistanceImage	(	const base::samples::frame::Frame &	image1,
		const base::samples::frame::Frame &	image2,
		const std::vector< Matcher::p_match > &	matches,
		const VisualOdometryStereo::parameters &	viso2param,
		base::samples::DistanceImage &	distImage
	)

Firs set all the distance z values to NaN

void StereoOdometer::createPointCloud	(	const Eigen::Affine3d &	tf,
		const base::samples::frame::Frame &	image1,
		const std::vector< Matcher::p_match > &	matches,
		const std::vector< int32_t > &	inlier_indices,
		const Eigen::Matrix4d &	Q,
		const Eigen::Affine3d &	deltaPose,
		boost::unordered_map< int32_t, int32_t > &	hashIdx,
		boost::circular_buffer< std::map< int32_t, HashPoint, std::less< int32_t >, Eigen::aligned_allocator< std::pair< const int32_t, HashPoint > > > > &	hashPointcloud
	)

Image for the colored points

Uncertainty in the images (left and right) planes

Create the hash table with the point cloud

3D Point

Point in the desired frame

Store it in the hash element

Color

Uncertainty information

Jacobian Matrix for the triangulation noise model

Covariance in the desired frame

Compute Jacobian

Add key to the hash Point cloud

Feature index

Assign the index hash

Order the point cloud by match indexes

void StereoOdometer::drawMatches	(	const base::samples::frame::Frame &	image1,
		const base::samples::frame::Frame &	image2,
		const std::vector< Matcher::p_match > &	matches,
		const std::vector< int32_t > &	inlier_indices,
		base::samples::frame::Frame &	imageOutput
	)

void StereoOdometer::errorHook ( )

This hook is called by Orocos when the component is in the RunTimeError state, at each activity step. See the discussion in updateHook() about triggering options.

Call recover() to go back in the Runtime state.

void StereoOdometer::left_frameTransformerCallback	(	const base::Time &	ts,
		const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &	left_frame_sample
	)

protectedvirtual

Debug intra frame image

Transformer transformation

Get the transformation (transformation) Tbody_left_camera which is body = Tbody_left_camera left_camera

The image need to be in gray scale and undistorted

Left color image

Check the time difference between inertial sensors and joint samples

If the difference in time is less than half of a period run the odometry

void StereoOdometer::postProcessPointCloud	(	boost::unordered_map< int32_t, int32_t > &	hashIdx,
		boost::circular_buffer< std::map< int32_t, HashPoint, std::less< int32_t >, Eigen::aligned_allocator< std::pair< const int32_t, HashPoint > > > > &	hashPointcloud,
		base::samples::Pointcloud &	pointcloud,
		std::vector< base::Matrix3d > &	pointsVar,
		std::vector< unsigned int > &	pointsIdx,
		base::MatrixXd &	deltaJacobCurr,
		base::MatrixXd &	deltaJacobPrev
	)

Resize variables to the number of elements in the current point cloud

The point to the point cloud

Get the idx on the previous point cloud

Look in the previous point cloud

Get the point at the previous point cloud

Get the current Jacobian

Get the previous Jacobian

Vector of indexes

Set the current Jacobian to Zero

Set the previous Jacobian to Zero

void StereoOdometer::right_frameTransformerCallback	(	const base::Time &	ts,
		const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &	right_frame_sample
	)

protectedvirtual

Transformer transformation

Get the transformation (transformation) Tbody_left_camera which is body = Tbody_left_camera left_camera

Correct distortion in image right

Check the time difference between inertial sensors and joint samples

If the difference in time is less than half of a period run the odometry

bool StereoOdometer::startHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to Running. If it returns false, then the component will stay in Stopped. Otherwise, it goes into Running and updateHook() will be called.

void StereoOdometer::stopHook ( )

This hook is called by Orocos when the state machine transitions from Running to Stopped after stop() has been called.

void StereoOdometer::updateHook ( )

This hook is called by Orocos when the component is in the Running state, at each activity step. Here, the activity gives the "ticks" when the hook should be called.

The error(), exception() and fatal() calls, when called in this hook, allow to get into the associated RunTimeError, Exception and FatalError states.

In the first case, updateHook() is still called, and recover() allows you to go back into the Running state. In the second case, the errorHook() will be called instead of updateHook(). In Exception, the component is stopped and recover() needs to be called before starting it again. Finally, FatalError cannot be recovered.

Friends And Related Function Documentation

friend class StereoOdometerBase

friend

Member Data Documentation

frame_helper::StereoCalibration viso2::StereoOdometer::cameracalib

protected

frame_helper::StereoCalibrationCv viso2::StereoOdometer::cameracalibCv

protected

Open Cv calibration for the perspective transformation Matrix Q

const int viso2::StereoOdometer::DEFAULT_CIRCULAR_BUFFER_SIZE = 2

staticprotected

RTT::extras::ReadOnlyPointer<base::samples::frame::Frame> viso2::StereoOdometer::frame_out

protected

Accumulated pose

frame_helper::FrameHelper viso2::StereoOdometer::frameHelperLeft

protected

Left and right images

frame_helper::FrameHelper viso2::StereoOdometer::frameHelperRight

protected

boost::unordered_map< int32_t, int32_t > viso2::StereoOdometer::hashIdx

protected

Error variance of image plane in pixel units

boost::circular_buffer< std::map < int32_t, HashPoint, std::less<int32_t>, Eigen::aligned_allocator< std::pair < const int32_t, HashPoint > > > > viso2::StereoOdometer::hashPointcloud

protected

current to previous index

boost::circular_buffer<base::samples::frame::FramePair> viso2::StereoOdometer::imagePair

protected

Pointer to Viso2 object

::base::samples::frame::Frame viso2::StereoOdometer::leftColorImage

protected

Re-projection matrix

Eigen::Affine3d viso2::StereoOdometer::pose

protected

Circular buffer of hash Point Cloud Output Port Variables

base::samples::RigidBodyState viso2::StereoOdometer::poseOut

protected

Accumulated pose

::base::Matrix2d viso2::StereoOdometer::pxleftVar

protected

coloring point clouds (if selected)

::base::Matrix2d viso2::StereoOdometer::pxrightVar

protected

Eigen::Matrix4d viso2::StereoOdometer::Q

protected

Frame helper

boost::shared_ptr<VisualOdometryStereo> viso2::StereoOdometer::viso

protected

VisualOdometryStereo::parameters viso2::StereoOdometer::viso2param

protected

The documentation for this class was generated from the following files:

/build/rock-master-18.09-perception-orogen-viso2-0.20150824/tasks/StereoOdometer.hpp
/build/rock-master-18.09-perception-orogen-viso2-0.20150824/tasks/StereoOdometer.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation