ModelFitting::ModelFitting() { cube2D = new BRICS_3D::PointCloud3D(); cube3D = new BRICS_3D::PointCloud3D(); cubeModelGenerator.setPointsOnEachSide(5); cubeModelGenerator.setCubeSideLength(0.057); cubeModelGenerator.setNumOfFaces(2); cubeModelGenerator.generatePointCloud(cube2D); cubeModelGenerator.setNumOfFaces(3); cubeModelGenerator.generatePointCloud(cube3D); Eigen::Matrix4f tempHomogenousMatrix; calculateHomogeneousMatrix(90,0,0,0,0,0,tempHomogenousMatrix,true); BRICS_3D::HomogeneousMatrix44* homogeneousTrans = new HomogeneousMatrix44( tempHomogenousMatrix(0), tempHomogenousMatrix(4), tempHomogenousMatrix(8), tempHomogenousMatrix(1), tempHomogenousMatrix(5), tempHomogenousMatrix(9), tempHomogenousMatrix(2), tempHomogenousMatrix(6), tempHomogenousMatrix(10), 0,0,0); cube2D->homogeneousTransformation(homogeneousTrans); cube3D->homogeneousTransformation(homogeneousTrans); ROS_INFO("Initialization Done...."); reliableScoreThreshold = 0.00008; bestScore = 1000; poseEstimatorICP = new BRICS_3D::SDK::IterativeClosestPoint(); }
void ModelFitting::kinectCloudCallback(const sensor_msgs::PointCloud2 &cloud){ bestTransformation = new Eigen::Matrix4f(); //Transforming Input message to BRICS_3D format pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_ptr(new pcl::PointCloud<pcl::PointXYZ>()); pcl::PointCloud<pcl::PointXYZ>::Ptr estimated_model_ptr(new pcl::PointCloud<pcl::PointXYZ>()); BRICS_3D::PointCloud3D *in_cloud = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *finalModel2D = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *finalModel3D = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *transformedCubeModel2D = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *transformedCubeModel3D = new BRICS_3D::PointCloud3D(); BRICS_3D::Centroid3D centroidEstimator; //Transform sensor_msgs::PointCloud2 msg to pcl::PointCloud pcl::fromROSMsg (cloud, *cloud_xyz_ptr); // cast PCL to BRICS_3D type pclTypecaster.convertToBRICS3DDataType(cloud_xyz_ptr, in_cloud); // ROS_INFO("Size of input cloud: %d ", in_cloud->getSize()); Eigen::Vector3d centroid3d = centroidEstimator.computeCentroid(in_cloud); float xTrans = centroid3d[0]; float yTrans = centroid3d[1]; float zTrans = centroid3d[2]; // ROS_INFO("Initial estimate \n\tTranslation-[x,y,z,]=[%f,%f,%f]",xTrans, yTrans, zTrans); //Translate the cube models which will be our initial estimate for ICP Eigen::Matrix4f tempHomogenousMatrix; calculateHomogeneousMatrix(0,0,0,xTrans,yTrans,zTrans,tempHomogenousMatrix,true); BRICS_3D::HomogeneousMatrix44* homogeneousTrans = new HomogeneousMatrix44( 1, 0, 0, 0, 1, 0, 0, 0, 1, xTrans,yTrans,zTrans); for(unsigned int i=0; i<cube2D->getSize();i++){ BRICS_3D::Point3D *tempPoint = new BRICS_3D::Point3D((*cube2D->getPointCloud())[i].getX(), (*cube2D->getPointCloud())[i].getY(), (*cube2D->getPointCloud())[i].getZ()); transformedCubeModel2D->addPoint(tempPoint); delete tempPoint; } transformedCubeModel2D->homogeneousTransformation(homogeneousTrans); for(unsigned int i=0; i<cube3D->getSize();i++){ BRICS_3D::Point3D *tempPoint = new BRICS_3D::Point3D((*cube3D->getPointCloud())[i].getX(), (*cube3D->getPointCloud())[i].getY(), (*cube3D->getPointCloud())[i].getZ()); transformedCubeModel3D->addPoint(tempPoint); delete tempPoint; } ROS_INFO("Resultant cloud size: %d", transformedCubeModel3D->getSize()); transformedCubeModel3D->homogeneousTransformation(homogeneousTrans); //Performing 2D model alignment poseEstimatorICP->setDistance(0.1); poseEstimatorICP->setMaxIterations(1000); poseEstimatorICP->setObjectModel(transformedCubeModel2D); poseEstimatorICP->estimateBestFit(in_cloud, finalModel2D); float score2D = poseEstimatorICP->getFitnessScore(); Eigen::Matrix4f transformation2D=poseEstimatorICP->getFinalTransformation(); //Performing 3D model alignment poseEstimatorICP->setObjectModel(transformedCubeModel3D); poseEstimatorICP->estimateBestFit(in_cloud, finalModel3D); float score3D = poseEstimatorICP->getFitnessScore(); Eigen::Matrix4f transformation3D=poseEstimatorICP->getFinalTransformation(); if(score2D<score3D){ //publish model estimated using two sided cube if(score2D < bestScore){ if(score2D > reliableScoreThreshold){ ROS_INFO("[%s] Approximate Model Found(2D)!! Object May Not be visible enough...", modelPublisher->getTopic().c_str()); } else { ROS_INFO("[%s] Reliable Model Found(2D) :) ", modelPublisher->getTopic().c_str()); } centroid3d = centroidEstimator.computeCentroid(finalModel2D); xtranslation=centroid3d[0]; ytranslation=centroid3d[1]; ztranslation=centroid3d[2]; *bestTransformation = transformation2D; bestScore = score2D; } pclTypecaster.convertToPCLDataType(estimated_model_ptr,finalModel2D); ROS_INFO("Best score found by 3D model : %f", score2D); } else { //publish model estimated using three sided cube if(score3D<bestScore){ if(score3D > reliableScoreThreshold){ ROS_INFO("[%s] Approximate Model Found(3D)!! Object May Not be visible enough...", modelPublisher->getTopic().c_str()); }else { ROS_INFO("[%s] Reliable Model Found(3D) :) ", modelPublisher->getTopic().c_str()); } centroid3d = centroidEstimator.computeCentroid(finalModel2D); xtranslation=centroid3d[0]; ytranslation=centroid3d[1]; ztranslation=centroid3d[2]; *bestTransformation = transformation3D; bestScore=score3D; } pclTypecaster.convertToPCLDataType(estimated_model_ptr,finalModel3D); ROS_INFO("Best score found by 3D model : %f", score3D); } double yRot = asin (-(*(bestTransformation))(2)); double xRot = asin ((*(bestTransformation))(6)/cos(yRot)); double zRot = asin ((*(bestTransformation))(1)/cos(yRot)); static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(xtranslation, ytranslation, ztranslation) ); //Todo stop using Quaternion transform.setRotation( tf::Quaternion(xRot, yRot, zRot) ); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/openni_rgb_optical_frame", modelPublisher->getTopic())); calculateHomogeneousMatrix(xRot,yRot,zRot,xtranslation,ytranslation,ztranslation,tempHomogenousMatrix,0); BRICS_3D::PointCloud3D *finalModel = new BRICS_3D::PointCloud3D(); for(unsigned int i=0; i<cube3D->getSize();i++){ BRICS_3D::Point3D *tempPoint = new BRICS_3D::Point3D((*cube3D->getPointCloud())[i].getX(), (*cube3D->getPointCloud())[i].getY(), (*cube3D->getPointCloud())[i].getZ()); finalModel->addPoint(tempPoint); delete tempPoint; } homogeneousTrans = new HomogeneousMatrix44( tempHomogenousMatrix(0), tempHomogenousMatrix(4), tempHomogenousMatrix(8), tempHomogenousMatrix(1), tempHomogenousMatrix(5), tempHomogenousMatrix(9), tempHomogenousMatrix(2), tempHomogenousMatrix(6), tempHomogenousMatrix(10), xtranslation,ytranslation,ztranslation); finalModel->homogeneousTransformation(homogeneousTrans); // pclTypecaster.convertToPCLDataType(estimated_model_ptr,finalModel); estimated_model_ptr->header.frame_id = "/openni_rgb_optical_frame"; modelPublisher->publish(*estimated_model_ptr); delete in_cloud; delete finalModel; delete finalModel2D; delete finalModel3D; delete transformedCubeModel2D; delete transformedCubeModel3D; delete bestTransformation; delete homogeneousTrans; }
PoseEstimation6D::PoseEstimation6D() { //default initialization of roi extractor this->hsvBasedRoiExtractor.setMinH(0); this->hsvBasedRoiExtractor.setMaxH(0); this->hsvBasedRoiExtractor.setMinS(0); this->hsvBasedRoiExtractor.setMaxS(0); //default initialization of cluster extractor this->euclideanClusterExtractor.setMinClusterSize(0); this->euclideanClusterExtractor.setMaxClusterSize(0); this->euclideanClusterExtractor.setClusterTolerance(0); poseEstimatorICP = new BRICS_3D::SDK::IterativeClosestPoint(); //default initialization of cube models cube2D = new BRICS_3D::PointCloud3D(); cube3D = new BRICS_3D::PointCloud3D(); cubeModelGenerator.setPointsOnEachSide(5); cubeModelGenerator.setCubeSideLength(0.06); cubeModelGenerator.setNumOfFaces(2); cubeModelGenerator.generatePointCloud(cube2D); cubeModelGenerator.setNumOfFaces(3); cubeModelGenerator.generatePointCloud(cube3D); Eigen::Matrix4f tempHomogenousMatrix; calculateHomogeneousMatrix(90,0,0,0,0,0,tempHomogenousMatrix,true); BRICS_3D::HomogeneousMatrix44* homogeneousTrans = new HomogeneousMatrix44( tempHomogenousMatrix(0), tempHomogenousMatrix(4), tempHomogenousMatrix(8), tempHomogenousMatrix(1), tempHomogenousMatrix(5), tempHomogenousMatrix(9), tempHomogenousMatrix(2), tempHomogenousMatrix(6), tempHomogenousMatrix(10), 0,0,0); cube2D->homogeneousTransformation(homogeneousTrans); cube3D->homogeneousTransformation(homogeneousTrans); ROS_INFO("Initialization Done...."); reliableScoreThreshold = 0.00008; maxNoOfObjects = 0; //ToDO initialize // bestTransformation[0]=0; // bestTransformation[1]=0; // bestTransformation[2]=0; // bestTransformation[3]=0; // bestTransformation[4]=0; // bestTransformation[5]=0; // bestTransformation[6]=0; // bestTransformation[7]=0; // bestTransformation[8]=0; // bestTransformation[9]=0; // bestTransformation[10]=0; // bestTransformation[11]=0; // bestTransformation[12]=0; // bestTransformation[13]=0; // bestTransformation[14]=0; // bestTransformation[15]=0; // translation[0] = 0; // translation[1] = 0; // translation[2] = 0; centroid3DEstimator = new BRICS_3D::Centroid3D(); }
void PoseEstimation6D::estimatePose(BRICS_3D::PointCloud3D *in_cloud, int objCount){ Eigen::Vector3d centroid3d = centroid3DEstimator->computeCentroid(in_cloud); float xTrans = centroid3d[0]; float yTrans = centroid3d[1]; float zTrans = centroid3d[2]; // ROS_INFO("Initial estimate \n\tTranslation-[x,y,z,]=[%f,%f,%f]",xTrans, yTrans, zTrans); //Translate the cube models which will be our initial estimate for ICP Eigen::Matrix4f tempHomogenousMatrix; calculateHomogeneousMatrix(0,0,0,xTrans,yTrans,zTrans,tempHomogenousMatrix,true); BRICS_3D::HomogeneousMatrix44* homogeneousTrans = new HomogeneousMatrix44( 1, 0, 0, 0, 1, 0, 0, 0, 1, xTrans,yTrans,zTrans); BRICS_3D::PointCloud3D *transformedCubeModel2D = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *transformedCubeModel3D = new BRICS_3D::PointCloud3D(); for(unsigned int i=0; i<cube2D->getSize();i++){ BRICS_3D::Point3D *tempPoint = new BRICS_3D::Point3D(cube2D->getPointCloud()->data()[i].getX(), cube2D->getPointCloud()->data()[i].getY(), cube2D->getPointCloud()->data()[i].getZ()); transformedCubeModel2D->addPoint(tempPoint); delete tempPoint; } transformedCubeModel2D->homogeneousTransformation(homogeneousTrans); for(unsigned int i=0; i<cube3D->getSize();i++){ BRICS_3D::Point3D *tempPoint = new BRICS_3D::Point3D(cube3D->getPointCloud()->data()[i].getX(), cube3D->getPointCloud()->data()[i].getY(), cube3D->getPointCloud()->data()[i].getZ()); transformedCubeModel3D->addPoint(tempPoint); delete tempPoint; } ROS_INFO("Resultant cloud size: %d", transformedCubeModel3D->getSize()); transformedCubeModel3D->homogeneousTransformation(homogeneousTrans); BRICS_3D::PointCloud3D *finalModel2D = new BRICS_3D::PointCloud3D(); BRICS_3D::PointCloud3D *finalModel3D = new BRICS_3D::PointCloud3D(); //Performing 2D model alignment poseEstimatorICP->setDistance(0.1); poseEstimatorICP->setMaxIterations(1000); poseEstimatorICP->setObjectModel(transformedCubeModel2D); poseEstimatorICP->estimateBestFit(in_cloud, finalModel2D); float score2D = poseEstimatorICP->getFitnessScore(); Eigen::Matrix4f transformation2D = poseEstimatorICP->getFinalTransformation(); //Performing 3D model alignment poseEstimatorICP->setObjectModel(transformedCubeModel3D); poseEstimatorICP->estimateBestFit(in_cloud, finalModel3D); float score3D = poseEstimatorICP->getFitnessScore(); Eigen::Matrix4f transformation3D = poseEstimatorICP->getFinalTransformation(); if(score2D<score3D){ //publish model estimated using two sided cube if(score2D < bestScore[objCount]){ if(score2D > reliableScoreThreshold){ ROS_INFO("[%s_%d] Approximate Model Found(2D)!! Object May Not be visible enough...", regionLabel.c_str(),objCount); } else { ROS_INFO("[%s_%d] Reliable Model Found(2D) :) ", regionLabel.c_str(), objCount); } *(bestTransformation[objCount]) = transformation2D; centroid3d = centroid3DEstimator->computeCentroid(finalModel2D); xtranslation[objCount]=centroid3d[0]; ytranslation[objCount]=centroid3d[1]; ztranslation[objCount]=centroid3d[2]; } ROS_INFO("[%s_%d] Best score found by 2D model : %f", regionLabel.c_str(), objCount,score2D); bestScore[objCount] = score2D; } else { //publish model estimated using three sided cube if(score3D<bestScore[objCount]){ if(score3D > reliableScoreThreshold){ ROS_INFO("[%s_%d] Approximate Model Found(3D)!! Object May Not be visible enough...", regionLabel.c_str(),objCount); }else { ROS_INFO("[%s_%d] Reliable Model Found(3D) :) ", regionLabel.c_str(), objCount); } *(bestTransformation[objCount]) = transformation3D; centroid3d = centroid3DEstimator->computeCentroid(finalModel3D); xtranslation[objCount]=centroid3d[0]; ytranslation[objCount]=centroid3d[1]; ztranslation[objCount]=centroid3d[2]; } ROS_INFO("[%s_%d] Best score found by 3D model : %f", regionLabel.c_str(), objCount, score3D); bestScore[objCount]=score3D; } double yRot = asin (-(*(bestTransformation[objCount]))(2)); double xRot = asin ((*(bestTransformation[objCount]))(6)/cos(yRot)); double zRot = asin ((*(bestTransformation[objCount]))(1)/cos(yRot)); // Eigen::Matrix4f tempHomogenousMatrix; // calculateHomogeneousMatrix(xRot, yRot, zRot, translation[0], translation[1], translation[2],tempHomogenousMatrix,0); static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(xtranslation[objCount], ytranslation[objCount], ztranslation[objCount]) ); //Todo stop using Quaternion transform.setRotation( tf::Quaternion(xRot, yRot, zRot) ); std::stringstream ss; ss << regionLabel << "_object_" << objCount; br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/openni_rgb_optical_frame", ss.str())); delete finalModel2D; delete finalModel3D; delete transformedCubeModel2D; delete transformedCubeModel3D; }