void ARSinglePublisher::camInfoCallback (const sensor_msgs::CameraInfoConstPtr & cam_info)
{
if (!getCamInfo_)
{
cam_info_ = (*cam_info);
cam_param_.xsize = cam_info_.width;
cam_param_.ysize = cam_info_.height;
cam_param_.mat[0][0] = cam_info_.P[0];
cam_param_.mat[1][0] = cam_info_.P[4];
cam_param_.mat[2][0] = cam_info_.P[8];
cam_param_.mat[0][1] = cam_info_.P[1];
cam_param_.mat[1][1] = cam_info_.P[5];
cam_param_.mat[2][1] = cam_info_.P[9];
cam_param_.mat[0][2] = cam_info_.P[2];
cam_param_.mat[1][2] = cam_info_.P[6];
cam_param_.mat[2][2] = cam_info_.P[10];
cam_param_.mat[0][3] = cam_info_.P[3];
cam_param_.mat[1][3] = cam_info_.P[7];
cam_param_.mat[2][3] = cam_info_.P[11];
cam_param_.dist_factor[0] = cam_info_.K[2]; // x0 = cX from openCV calibration
cam_param_.dist_factor[1] = cam_info_.K[5]; // y0 = cY from openCV calibration
cam_param_.dist_factor[2] = -100*cam_info_.D[0]; // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
cam_param_.dist_factor[3] = 1.0; // scale factor, should probably be >1, but who cares...
arInit();
ROS_INFO ("Subscribing to image topic");
cam_sub_ = it_.subscribe (cameraImageTopic_, 1, &ARSinglePublisher::getTransformationCallback, this);
getCamInfo_ = true;
}
}
void
compInit(void)
{
int gclevel;
gclevel = compDoGc ? StoCtl_GcLevel_Automatic :
compDoGcFile ? StoCtl_GcLevel_Demand :
StoCtl_GcLevel_Never;
/* If and when to GG. */
stoCtl(StoCtl_GcLevel, gclevel);
if (compDoGcVerbose)
/* Put GC messages on osStdout. */
stoCtl(StoCtl_GcFile, osStdout);
else
stoCtl(StoCtl_GcFile, (FILE *) NULL);
/* Do not initialize pieces. */
stoCtl(StoCtl_Wash, false);
obInit();
dbInit();
comsgOpen();
compInfoAudit();
osSetBreakHandler(compSignalHandler);
osSetFaultHandler(compSignalHandler);
osSetLimitHandler(compSignalHandler);
osSetDangerHandler(compSignalHandler);
exitSetHandler (compExitHandler);
fileSetHandler (compFileError);
stoSetHandler (compStoreError);
sxiSetHandler (compSExprError);
pathInit();
if (compRootDir) {
fileAddLibraryDirectory(fileSubdir(compRootDir, "lib"));
fileAddLibraryDirectory(fileSubdir(fileSubdir(compRootDir, "share"),
"lib"));
fileAddIncludeDirectory(fileSubdir(compRootDir, "include"));
ccSetRoot(compRootDir);
}
fileAddLibraryDirectory(osCurDirName());
compCfgInit(compRootDir);
arInit(compLibraryFiles, compLibraryKeys);
sxiInit();
keyInit();
ssymInit();
stabInitGlobal();
tfInit();
fmttsInit();
foamInit();
optInit();
tinferInit();
}
/*
* One and only one callback, now takes cloud, does everything else needed.
*/
void ARPublisher::getTransformationCallback (const sensor_msgs::PointCloud2ConstPtr & msg)
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k, j;
/* do we need to initialize? */
if(!configured_)
{
if(msg->width == 0 || msg->height == 0)
{
ROS_ERROR ("Deformed cloud! Size = %d, %d.", msg->width, msg->height);
return;
}
cam_param_.xsize = msg->width;
cam_param_.ysize = msg->height;
cam_param_.dist_factor[0] = msg->width/2; // x0 = cX from openCV calibration
cam_param_.dist_factor[1] = msg->height/2; // y0 = cY from openCV calibration
cam_param_.dist_factor[2] = 0; // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
cam_param_.dist_factor[3] = 1.0; // scale factor, should probably be >1, but who cares...
arInit ();
}
/* convert cloud to PCL */
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
/* get an OpenCV image from the cloud */
pcl::toROSMsg (cloud, *image_msg);
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
}
dataPtr = (ARUint8 *) cv_ptr->image.ptr();
/* detect the markers in the video frame */
if (arDetectMarkerLite (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
argCleanup ();
return;
}
arPoseMarkers_.markers.clear ();
/* check for known patterns */
for (i = 0; i < objectnum; i++)
{
k = -1;
for (j = 0; j < marker_num; j++)
{
if (object[i].id == marker_info[j].id)
{
if (k == -1)
k = j;
else // make sure you have the best pattern (highest confidence factor)
if (marker_info[k].cf < marker_info[j].cf)
k = j;
}
}
if (k == -1)
{
object[i].visible = 0;
continue;
}
/* create a cloud for marker corners */
int d = marker_info[k].dir;
PointCloud marker;
marker.push_back( cloud.at( (int)marker_info[k].vertex[(4-d)%4][0], (int)marker_info[k].vertex[(4-d)%4][1] ) ); // upper left
marker.push_back( cloud.at( (int)marker_info[k].vertex[(5-d)%4][0], (int)marker_info[k].vertex[(5-d)%4][1] ) ); // upper right
marker.push_back( cloud.at( (int)marker_info[k].vertex[(6-d)%4][0], (int)marker_info[k].vertex[(6-d)%4][1] ) ); // lower right
marker.push_back( cloud.at( (int)marker_info[k].vertex[(7-d)%4][0], (int)marker_info[k].vertex[(7-d)%4][1] ) );
/* create an ideal cloud */
double w = object[i].marker_width;
PointCloud ideal;
ideal.push_back( makeRGBPoint(-w/2,w/2,0) );
ideal.push_back( makeRGBPoint(w/2,w/2,0) );
ideal.push_back( makeRGBPoint(w/2,-w/2,0) );
ideal.push_back( makeRGBPoint(-w/2,-w/2,0) );
/* get transformation */
Eigen::Matrix4f t;
TransformationEstimationSVD obj;
obj.estimateRigidTransformation( marker, ideal, t );
/* get final transformation */
tf::Transform transform = tfFromEigen(t.inverse());
// any(transform == nan)
tf::Matrix3x3 m = transform.getBasis();
tf::Vector3 p = transform.getOrigin();
bool invalid = false;
for(int i=0; i < 3; i++)
for(int j=0; j < 3; j++)
invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0);
for(int i=0; i < 3; i++)
invalid = (invalid || isnan(p[i]));
if(invalid)
continue;
/* publish the marker */
ar_pose::ARMarker ar_pose_marker;
ar_pose_marker.header.frame_id = msg->header.frame_id;
ar_pose_marker.header.stamp = msg->header.stamp;
ar_pose_marker.id = object[i].id;
ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX();
ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY();
ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ();
ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX();
ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY();
ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ();
ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW();
ar_pose_marker.confidence = marker_info->cf;
arPoseMarkers_.markers.push_back (ar_pose_marker);
/* publish transform */
if (publishTf_)
{
broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name));
}
/* publish visual marker */
if (publishVisualMarkers_)
{
tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = transform * m; // marker pose in the camera frame
tf::poseTFToMsg (markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = msg->header.frame_id;
rvizMarker_.header.stamp = msg->header.stamp;
rvizMarker_.id = object[i].id;
rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
switch (i)
{
case 0:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 1.0f;
rvizMarker_.color.a = 1.0;
break;
case 1:
rvizMarker_.color.r = 1.0f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
break;
default:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
}
rvizMarker_.lifetime = ros::Duration ();
rvizMarkerPub_.publish (rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
arMarkerPub_.publish (arPoseMarkers_);
ROS_DEBUG ("Published ar_multi markers");
}
int main(int argc, char *argv[])
{
printf("Debut initialisation\n");
/// Chargement des objets
//on ne stocke plus dans des mesh, mais dans un tableau possible de mesh à charger. L'id du meche à charger par le patron correspond
//à l'indice dans le tableau de mesh
mesh.push_back(new MeshObj("Others\\legoTexture.obj",NULL));
mesh.push_back(new MeshObj("Others\\brique_lego.obj", NULL));
printf("Chargement des objets réussi\n");
/// Initialisation de glut
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glClearColor(0, 0, 0, 0);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glShadeModel(GL_SMOOTH);
/// Initialisation d'ARToolKit et de la fenetre + appel boucle infini
arInit();
arVideoCapStart();
glutPositionWindow((glutGet(GLUT_SCREEN_WIDTH)-cparam.xsize)/2, (glutGet(GLUT_SCREEN_HEIGHT)-cparam.ysize)/2);
glutReshapeFunc(resize);
glutMotionFunc(mouseMove);
//init du menu d'aide (aide mouvement)
menu.addBoutton("img\\delete.png",true,0,cparam.ysize-75,75,cparam.ysize);
menu.addBoutton("img\\move.png",true,75,cparam.ysize-75,150,cparam.ysize);
menu.addBoutton("img\\resize.png",true,150,cparam.ysize-75,225,cparam.ysize);
//init bouttons help/scan
//quit
quit.addBoutton("img\\quit.png",true,cparam.xsize-120,30,cparam.xsize-16,54+30,true);
quit.addBoutton("img\\quit1.png",true,cparam.xsize-120,30,cparam.xsize-16,54+30,false);
difQuit=differ(2000);
//help;
help.addBoutton("img\\aide1.png",true, cparam.xsize-120,54+35,cparam.xsize-16,54+54+35,true);
help.addBoutton("img\\aide2.png",true, cparam.xsize-120,54+35,cparam.xsize-16,54+54+35,false); //activé
help.addBoutton("img\\aide3.png",true, cparam.xsize-120,54+35,cparam.xsize-16,54+54+35,false); //selectioné
difAide=differ(2000);
menuShow=false;
//scan
scan.addBoutton("img\\scan5.png",true,cparam.xsize-120,54+40+54,cparam.xsize-16,54+54+54+40,true);
scan.addBoutton("img\\scan6.png",true,cparam.xsize-120,54+40+54,cparam.xsize-16,54+54+54+40,false);
scan.addBoutton("img\\scan7.png",true,cparam.xsize-120,54+40+54,cparam.xsize-16,54+54+54+40,false);
difScan=differ(2000);
/*FMOD_System_Create(&systemSon);
FMOD_System_Init(systemSon, 2, FMOD_INIT_NORMAL, NULL);
if(!FMOD_System_CreateSound(systemSon, "Data\\mouseclickDown.wav", FMOD_CREATESAMPLE, 0, &clickDown)) printf("chargement son: ok\n");
else printf("chargement son: echec\n");
if(!FMOD_System_CreateSound(systemSon, "Data\\mouseclickUp.wav", FMOD_CREATESAMPLE, 0, &clickUP)) printf("chargement son: ok\n");
else printf("chargement son: echec\n");*/
difIndex=differ(2000);
difMajeur=differ(2000);
printf("Fin initialisation\n");
argMainLoop(mouseClick, key, mainLoop);
return EXIT_SUCCESS;
}
