Skip to content

Commit

Permalink
compiles under groovy. bullet stuff switched to tf
Browse files Browse the repository at this point in the history
  • Loading branch information
Jonathan Binney committed Jan 16, 2013
1 parent b3073b0 commit fc262e6
Show file tree
Hide file tree
Showing 8 changed files with 119 additions and 110 deletions.
2 changes: 1 addition & 1 deletion .gitignore
View file
Expand Up @@ -8,4 +8,4 @@
*bin
*~
src/ar_track_alvar

*.user
3 changes: 2 additions & 1 deletion include/MultiMarker.h
View file
Expand Up @@ -36,6 +36,7 @@
#include "Camera.h"
#include "Filter.h"
#include "FileFormat.h"
#include <tf/LinearMath/Vector3.h>

namespace alvar {

Expand All @@ -61,7 +62,7 @@ class ALVAR_EXPORT MultiMarker {
std::map<int, CvPoint3D64f> pointcloud;
std::vector<int> marker_indices; // The marker id's to be used in marker field (first being the base)
std::vector<int> marker_status; // 0: not in point cloud, 1: in point cloud, 2: used in GetPose()
std::vector< std::vector<btVector3> > rel_corners; //The coords of the master marker relative to each child marker in marker_indices
std::vector< std::vector<tf::Vector3> > rel_corners; //The coords of the master marker relative to each child marker in marker_indices

int pointcloud_index(int marker_id, int marker_corner, bool add_if_missing=false);
int get_id_index(int id, bool add_if_missing=false);
Expand Down
35 changes: 17 additions & 18 deletions nodes/FindMarkerBundles.cpp
View file
Expand Up @@ -63,8 +63,7 @@
#include <pcl/filters/extract_indices.h>
#include <boost/lexical_cast.hpp>

#include <LinearMath/btMatrix3x3.h>
#include <LinearMath/btTransform.h>
#include <tf/tf.h>
#include <Eigen/Core>
#include <ar_track_alvar/kinect_filtering.h>
#include <ar_track_alvar/medianFilter.h>
Expand Down Expand Up @@ -255,7 +254,7 @@ int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_
//Grab the precomputed corner coords and correct for the weird Alvar coord system
for(int j = 0; j < 4; ++j)
{
btVector3 corner_coord = master.rel_corners[index][j];
tf::Vector3 corner_coord = master.rel_corners[index][j];
gm::PointStamped p, output_p;
p.header.frame_id = marker_frame;
p.point.x = corner_coord.y()/100.0;
Expand Down Expand Up @@ -679,17 +678,17 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
//p1-->p2 should point in Alvar's pos Y direction
int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
const CvPoint3D64f& p2, const CvPoint3D64f& p3,
btTransform &retT)
tf::Transform &retT)
{
const btVector3 q0(p0.x, p0.y, p0.z);
const btVector3 q1(p1.x, p1.y, p1.z);
const btVector3 q2(p2.x, p2.y, p2.z);
const btVector3 q3(p3.x, p3.y, p3.z);
const tf::Vector3 q0(p0.x, p0.y, p0.z);
const tf::Vector3 q1(p1.x, p1.y, p1.z);
const tf::Vector3 q2(p2.x, p2.y, p2.z);
const tf::Vector3 q3(p3.x, p3.y, p3.z);

// (inverse) matrix with the given properties
const btVector3 v = (q1-q0).normalized();
const btVector3 w = (q2-q1).normalized();
const btVector3 n = v.cross(w);
const tf::Vector3 v = (q1-q0).normalized();
const tf::Vector3 w = (q2-q1).normalized();
const tf::Vector3 n = v.cross(w);
btMatrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
m = m.inverse();

Expand All @@ -711,15 +710,15 @@ int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
btScalar ey = eig_quat.y();
btScalar ez = eig_quat.z();
btScalar ew = eig_quat.w();
btQuaternion quat(ex,ey,ez,ew);
tf::Quaternion quat(ex,ey,ez,ew);
quat = quat.normalized();

double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
btVector3 origin (qx,qy,qz);
tf::Vector3 origin (qx,qy,qz);

btTransform tform (quat, origin); //transform from master to marker
tf::Transform tform (quat, origin); //transform from master to marker
retT = tform;

return 0;
Expand All @@ -731,7 +730,7 @@ int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
int calcAndSaveMasterCoords(MultiMarkerBundle &master)
{
int mast_id = master.master_id;
std::vector<btVector3> rel_corner_coords;
std::vector<tf::Vector3> rel_corner_coords;

//Go through all the markers associated with this bundle
for (size_t i=0; i<master.marker_indices.size(); i++){
Expand All @@ -745,7 +744,7 @@ int calcAndSaveMasterCoords(MultiMarkerBundle &master)
}

//Use them to find a transform from the master frame to the child frame
btTransform tform;
tf::Transform tform;
makeMasterTransform(mark_corners[0], mark_corners[1], mark_corners[2], mark_corners[3], tform);

//Finally, find the coords of the corners of the master in the child frame
Expand All @@ -756,8 +755,8 @@ int calcAndSaveMasterCoords(MultiMarkerBundle &master)
double py = corner_coord.y;
double pz = corner_coord.z;

btVector3 corner_vec (px, py, pz);
btVector3 ans = (tform.inverse()) * corner_vec;
tf::Vector3 corner_vec (px, py, pz);
tf::Vector3 ans = (tform.inverse()) * corner_vec;
rel_corner_coords.push_back(ans);
}

Expand Down
36 changes: 18 additions & 18 deletions nodes/FindMarkerBundlesNoKinect.cpp
View file
Expand Up @@ -40,10 +40,11 @@
#include "MultiMarkerBundle.h"
#include "MultiMarkerInitializer.h"
#include "Shared.h"
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>.h>
#include <ar_track_alvar/AlvarMarker.h>
#include <ar_track_alvar/AlvarMarkers.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/image_encodings.h>

using namespace alvar;
using namespace std;
Expand All @@ -53,8 +54,7 @@ using namespace std;
#define GHOST_MARKER 3

Camera *cam;
IplImage *capture_;
sensor_msgs::CvBridge bridge_;
cv_bridge::CvImagePtr cv_ptr_;
image_transport::Subscriber cam_sub_;
ros::Publisher arMarkerPub_;
ros::Publisher rvizMarkerPub_;
Expand Down Expand Up @@ -112,13 +112,13 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_
qw = p.quaternion[0];

//Get the marker pose in the camera frame
btQuaternion rotation (qx,qy,qz,qw);
btVector3 origin (px,py,pz);
btTransform t (rotation, origin); //transform from cam to marker
tf::Quaternion rotation (qx,qy,qz,qw);
tf::Vector3 origin (px,py,pz);
tf::Transform t (rotation, origin); //transform from cam to marker

btVector3 markerOrigin (0, 0, 0);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m;
tf::Vector3 markerOrigin (0, 0, 0);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = t * m;

//Publish the cam to marker transform for main marker in each bundle
if(type==MAIN_MARKER){
Expand Down Expand Up @@ -191,12 +191,6 @@ void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_
//Callback to handle getting video frames and processing them
void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
if(init){
CvSize sz_ = cvSize (cam->x_res, cam->y_res);
capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
init = false;
}

//If we've already gotten the cam info, then go ahead
if(cam->getCamInfo_){
try{
Expand All @@ -214,11 +208,17 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
ar_track_alvar::AlvarMarker ar_pose_marker;
arPoseMarkers_.markers.clear ();


//Convert the image
capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);

//Get the estimated pose of the main markers by using all the markers in each bundle
GetMultiMarkerPoses(capture_);

// GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
// us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
// do this conversion here -jbinney
IplImage ipl_image = cv_ptr_->image;
GetMultiMarkerPoses(&ipl_image);

//Draw the observed markers that are visible and note which bundles have at least 1 marker seen
for(int i=0; i<n_bundles; i++)
Expand Down Expand Up @@ -267,7 +267,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
//Publish the marker messages
arMarkerPub_.publish (arPoseMarkers_);
}
catch (sensor_msgs::CvBridgeException & e){
catch (cv_bridge::Exception& e){
ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
}
}
Expand Down
41 changes: 22 additions & 19 deletions nodes/IndividualMarkers.cpp
View file
Expand Up @@ -38,10 +38,11 @@
#include "CvTestbed.h"
#include "MarkerDetector.h"
#include "Shared.h"
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <ar_track_alvar/AlvarMarker.h>
#include <ar_track_alvar/AlvarMarkers.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/image_encodings.h>

namespace gm=geometry_msgs;
namespace ata=ar_track_alvar;
Expand All @@ -55,8 +56,7 @@ using boost::make_shared;

bool init=true;
Camera *cam;
IplImage *capture_;
sensor_msgs::CvBridge bridge_;
cv_bridge::CvImagePtr cv_ptr_;
image_transport::Subscriber cam_sub_;
ros::Subscriber cloud_sub_;
ros::Publisher arMarkerPub_;
Expand Down Expand Up @@ -310,12 +310,6 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);

if(init){
CvSize sz_ = cvSize (cam->x_res, cam->y_res);
capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
init = false;
}

//If we've already gotten the cam info, then go ahead
if(cam->getCamInfo_){
//Convert cloud to PCL
Expand All @@ -326,13 +320,22 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
pcl::toROSMsg (cloud, *image_msg);
image_msg->header.stamp = msg->header.stamp;
image_msg->header.frame_id = msg->header.frame_id;



//Convert the image
capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);

//Get the estimated pose of the main markers by using all the markers in each bundle

// GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
// us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
// do this conversion here -jbinney
IplImage ipl_image = cv_ptr_->image;


//Use the kinect to improve the pose
Pose ret_pose;
GetMarkerPoses(capture_, cloud);
GetMarkerPoses(&ipl_image, cloud);

try{
tf::StampedTransform CamToOutput;
Expand All @@ -359,12 +362,12 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
double qz = p.quaternion[3];
double qw = p.quaternion[0];

btQuaternion rotation (qx,qy,qz,qw);
btVector3 origin (px,py,pz);
btTransform t (rotation, origin);
btVector3 markerOrigin (0, 0, 0);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
tf::Quaternion rotation (qx,qy,qz,qw);
tf::Vector3 origin (px,py,pz);
tf::Transform t (rotation, origin);
tf::Vector3 markerOrigin (0, 0, 0);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = t * m; // marker pose in the camera frame

//Publish the transform from the camera to the marker
std::string markerFrame = "ar_marker_";
Expand Down Expand Up @@ -442,7 +445,7 @@ void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
}
arMarkerPub_.publish (arPoseMarkers_);
}
catch (sensor_msgs::CvBridgeException & e){
catch (cv_bridge::Exception& e){
ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
}
}
Expand Down
40 changes: 22 additions & 18 deletions nodes/IndividualMarkersNoKinect.cpp
View file
Expand Up @@ -38,18 +38,18 @@
#include "CvTestbed.h"
#include "MarkerDetector.h"
#include "Shared.h"
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <ar_track_alvar/AlvarMarker.h>
#include <ar_track_alvar/AlvarMarkers.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/image_encodings.h>

using namespace alvar;
using namespace std;

bool init=true;
Camera *cam;
IplImage *capture_;
sensor_msgs::CvBridge bridge_;
cv_bridge::CvImagePtr cv_ptr_;
image_transport::Subscriber cam_sub_;
ros::Publisher arMarkerPub_;
ros::Publisher rvizMarkerPub_;
Expand All @@ -71,12 +71,6 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);

void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
if(init){
CvSize sz_ = cvSize (cam->x_res, cam->y_res);
capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
init = false;
}

//If we've already gotten the cam info, then go ahead
if(cam->getCamInfo_){
try{
Expand All @@ -89,8 +83,18 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
ROS_ERROR("%s",ex.what());
}

capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
marker_detector.Detect(capture_, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true);

//Convert the image
cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);

//Get the estimated pose of the main markers by using all the markers in each bundle

// GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
// us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
// do this conversion here -jbinney
IplImage ipl_image = cv_ptr_->image;

marker_detector.Detect(&ipl_image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true);

arPoseMarkers_.markers.clear ();
for (size_t i=0; i<marker_detector.markers->size(); i++)
Expand All @@ -106,12 +110,12 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
double qz = p.quaternion[3];
double qw = p.quaternion[0];

btQuaternion rotation (qx,qy,qz,qw);
btVector3 origin (px,py,pz);
btTransform t (rotation, origin);
btVector3 markerOrigin (0, 0, 0);
btTransform m (btQuaternion::getIdentity (), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
tf::Quaternion rotation (qx,qy,qz,qw);
tf::Vector3 origin (px,py,pz);
tf::Transform t (rotation, origin);
tf::Vector3 markerOrigin (0, 0, 0);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = t * m; // marker pose in the camera frame

//Publish the transform from the camera to the marker
std::string markerFrame = "ar_marker_";
Expand Down Expand Up @@ -189,7 +193,7 @@ void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
}
arMarkerPub_.publish (arPoseMarkers_);
}
catch (sensor_msgs::CvBridgeException & e){
catch (cv_bridge::Exception& e){
ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
}
}
Expand Down

0 comments on commit fc262e6

Please sign in to comment.