Visual Odometry - clems4ever/ardrone_loclib GitHub Wiki
Overview
The goal here is to perform mono visual odometry from the Parrot AR.Drone's front camera under ROS Fuerte. We are going to use viso2_ros, a ros wrapper of libviso2 available here: http://wiki.ros.org/viso2_ros
Libviso2 states the constraint for mono odometry to work fine that camera pitch and height from the ground need to be fixed, which can be assumed for the drone as long as we use a constant flight altitude.
Dependencies
- A configured installation of ROS Fuerte and a set up workspace: http://wiki.ros.org/fuerte
- ardrone_autonomy: http://wiki.ros.org/ardrone_autonomy
- image_proc: http://wiki.ros.org/image_proc?distro=fuerte
- vision_opencv: http://wiki.ros.org/vision_opencv?distro=fuerte
- rviz: http://wiki.ros.org/rviz?distro=fuerte
Installation
Basic instructions to install a ROS package under ROS Fuerte (not provided on viso2_ros wiki).
roscd
git clone https://github.com/srv/viso2 -b fuerte
rosws set viso2
. setup.bash
rosmake viso2
Quick run with rviz
A launch file to run viso2_ros and other packages with used parameters are provided here.
cd [...]/ardrone_loclib/visual_odometry
roslaunch quick_run.launch
Modification for our project
As the pose covariance is not published, we are going to make a quick fix (though dirty) to make viso2_ros data valid for a Kalman filter (explained in another part). These changes are allowed as viso2_ros is under GPL license.
Constant visual covariance
The visual covariance used is constant (here: 0.33).
Patch
This has to be done on the original viso2 directory.
roscd
patch -p0 < [...]/ardrone_loclib/visual_odometry/constant.patch
rosmake viso2
Details
Add lines in viso2/viso2_ros/src/odometer_base.h.
[...]
#include <boost/assign/list_of.hpp>
[...]
void integrateAndPublish(const tf::Tranform& delta_transform, const ros::Time& timestamp)
{
[...]
//odometry_msg.pose.covariance = pose_covariance_;
//odometry_msg.twist.covariance = twist_covariance_;
odometry_msg.pose.covariance = boost::assign::list_of(0.33)(0)(0)(0)(0)(0)
(0)(0.33)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(0.33);
odometry_msg.twist.covariance = boost::assign::list_of(0.33)(0)(0)(0)(0)(0)
(0)(0.33)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(0.33);
odom_pub_.publish(odometry_msg);
[...]
}
[...]
Visual covariance as a ROS parameter
Here, visual covariance can be set as a ROS parameter: visual_covariance.
Patch
This also has to be done on the original viso2 directory.
If it has been patched with constant.patch, first reverse it.
roscd
patch -p0 -R < [...]/ardrone_loclib/visual_odometry/constant.patch
Then
roscd
patch -p0 < [...]/ardrone_loclib/visual_odometry/parameter.patch
rosmake viso2
Details
Add lines in viso2/viso2_ros/src/odometer_base.h.
[...]
#include <boost/assign/list_of.hpp>
[...]
private:
[...]
// custom covariance
double visual_covariance_;
[...]
public:
OdometerBase()
{
[...]
local_nh.param("visual_covariance", visual_covariance_, 0.0);
ROS_INFO_STREAM("[...]" << std::endl <<
" visual_covariance = " << visual_covariance_);
[...]
}
[...]
void integrateAndPublish(const tf::Tranform& delta_transform, const ros::Time& timestamp)
{
[...]
//odometry_msg.pose.covariance = pose_covariance_;
//odometry_msg.twist.covariance = twist_covariance_;
odometry_msg.pose.covariance = boost::assign::list_of(visual_covariance_)(0)(0)(0)(0)(0)
(0)(visual_covariance_)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(visual_covariance_);
odometry_msg.twist.covariance = boost::assign::list_of(visual_covariance_)(0)(0)(0)(0)(0)
(0)(visual_covariance_)(0)(0)(0)(0)
(0)(0)(999999)(0)(0)(0)
(0)(0)(0)(999999)(0)(0)
(0)(0)(0)(0)(999999)(0)
(0)(0)(0)(0)(0)(visual_covariance_);
odom_pub_.publish(odometry_msg);
[...]
}
[...]
Add lines in viso2/viso2_ros/src/odometry_params.h.
[...]
/// loads common and mono specific params
void loadParams(const ros::NodeHandle& local_nh, VisualOdometryMono::parameters& params)
{
[...]
if (!local_nh.getParam("visual_covariance", params.visual_covariance))
{
ROS_WARN("Parameter 'visual_covariance' is required but not set. Using default: %f", params.visual_covariance);
}
} // end of namespace
[...]
std::ostream& operator<<(std::ostream& out, const VisualOdometryMono::parameters& params)
{
[...]
out << " visual_covariance = " << params.visual_covariance << std::endl;
return out;
}
[...]
Add lines in viso2/libviso2/libviso2/src/viso_mono.h.
[...]
public:
// monocular-specific parameters (mandatory: height,pitch)
struct parameters : public VisualOdometry::parameters {
[...]
double visual_covariance; // custom visual covariance. Used because of covariance publication issues in ROS
parameters () {
[...]
visual_covariance = 0.33; // initialization value if not set
}
};
[...]
Troubleshooting
-
"package/stack viso2_ros depends on non-existent package cv_bridge" : dependency on vision_opencv is unmet. Run
sudo apt-get install ros-fuerte-vision-opencv. -
"rosws : command not found" : install rosinstall package with
sudo apt-get install python-rosinstall. -
"Reversed (or previously applied) patch detected!" : Before applying a new patch, reverse the previously applied one with
patch -p0 -R < path_to_previous_patch.