00001
00021 #include "pose_validation.h"
00022
00023 #include <rviz/mesh_loader.h>
00024
00025 #include <OgreMesh.h>
00026 #include <OGRE/OgreLight.h>
00027
00028 #include <ros/package.h>
00029 #include <cmath>
00030 #include <ros/console.h>
00031
00032 namespace descriptor_surface_based_recognition {
00033
00034 PoseValidation::PoseValidation() : is_initialized_(false)
00035 {
00036
00037 }
00038
00039 PoseValidation::PoseValidation(double image_width, double image_height, double far_plane, double near_plane, double cx, double cy, double fx, double fy, int render_width, int render_height) :
00040 image_width_(image_width),
00041 image_height_(image_height),
00042 is_initialized_(true)
00043 {
00044
00045 ROS_DEBUG_STREAM("Initialize render system");
00046
00047 render_sys_ = rviz::RenderSystem::get();
00048 root_ = render_sys_->root();
00049 if (!root_->isInitialised()) {
00050 root_->initialise(false);
00051 }
00052
00053 ROS_DEBUG_STREAM("Render system initialized");
00054
00055 scene_manager_ = root_->createSceneManager(Ogre::ST_GENERIC);
00056
00057 scene_manager_->setAmbientLight(Ogre::ColourValue(0.5, 0.5, 0.5));
00058
00059 Ogre::Light* light2 = scene_manager_->createLight("MainLight");
00060 light2->setType( Ogre::Light::LT_DIRECTIONAL );
00061 light2->setDirection( Ogre::Vector3( 0 , 0, -1 ) );
00062 light2->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) );
00063
00064
00065 camera_ = scene_manager_->createCamera("mainCamera");
00066
00067 Ogre::Matrix4 proj_matrix;
00068 proj_matrix = Ogre::Matrix4::ZERO;
00069
00070 proj_matrix[0][0] = 2.0 * fx/image_width;
00071 proj_matrix[1][1] = 2.0 * fy/image_height;
00072 proj_matrix[0][2] = 2.0 * (0.5 - cx/image_width);
00073 proj_matrix[1][2] = 2.0 * (cy/image_height - 0.5);
00074 proj_matrix[2][2] = -(far_plane + near_plane) / (far_plane - near_plane);
00075 proj_matrix[2][3] = -2.0 * far_plane * near_plane / (far_plane - near_plane);
00076 proj_matrix[3][2] = -1;
00077 camera_->setCustomProjectionMatrix(true, proj_matrix);
00078
00079 camera_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00080 camera_node_->attachObject(camera_);
00081
00082
00083 rtt_texture_ =
00084 Ogre::TextureManager::getSingleton().createManual(
00085 "RttTex",
00086 Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00087 Ogre::TEX_TYPE_2D,
00088 image_width, image_height,
00089 0,
00090 Ogre::PF_R8G8B8,
00091 Ogre::TU_RENDERTARGET);
00092
00093
00094 render_texture_ = rtt_texture_->getBuffer()->getRenderTarget();
00095
00096 render_texture_->addViewport(camera_);
00097 render_texture_->getViewport(0)->setClearEveryFrame(true);
00098 render_texture_->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Blue);
00099 render_texture_->getViewport(0)->setOverlaysEnabled(false);
00100
00101 render_texture_->update();
00102
00103 ROS_DEBUG_STREAM("Create render window");
00104
00105 window_ = root_->createRenderWindow("Pose validation render window", render_width, render_height, false);
00106
00107 Ogre::WindowEventUtilities::messagePump();
00108 window_->addViewport(camera_);
00109 }
00110
00111 bool PoseValidation::isInitialized() const
00112 {
00113 return is_initialized_;
00114 }
00115
00116 HalconCpp::HImage PoseValidation::renderObjectImage(RecognitionResult *recognition_result, Ogre::MeshPtr mesh)
00117 {
00118
00119 HalconCpp::HPose pose;
00120 pose.SetFromTuple(recognition_result->getPose());
00121 HalconCpp::HQuaternion quaternion;
00122 quaternion.PoseToQuat(pose);
00123 quaternion = quaternion.QuatCompose(recognition_result->getAdjustedRotation());
00124
00125 scene_manager_->destroyAllEntities();
00126
00127 Ogre::Entity* meshEntity = scene_manager_->createEntity(mesh->getName());
00128 Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode(Ogre::Vector3(pose.ConvertToTuple()[0], -1 * pose.ConvertToTuple()[1], -1 * pose.ConvertToTuple()[2]));
00129 node->scale(0.001, 0.001, 0.001);
00130 node->attachObject(meshEntity);
00131 Ogre::Quaternion orient(quaternion.ConvertToTuple()[0], quaternion.ConvertToTuple()[1], quaternion.ConvertToTuple()[2], quaternion.ConvertToTuple()[3]);
00132 orient = Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X) * orient;
00133 node->setOrientation(orient);
00134
00135 root_->renderOneFrame();
00136 render_texture_->update();
00137
00138 rtt_texture_->getBuffer()->lock(Ogre::HardwareBuffer::HBL_NORMAL);
00139
00140 const Ogre::PixelBox& pixelBox = rtt_texture_->getBuffer()->getCurrentLock();
00141
00142 Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
00143 long* pixeldata = (long*)const_cast<unsigned char*>(&pDest[0]);
00144 rtt_texture_->getBuffer()->unlock();
00145
00146 HalconCpp::HImage img;
00147 img.GenImageInterleaved(pixeldata, "bgrx", image_width_, image_height_, 0, "byte", image_width_, image_height_, 0, 0, -1, 0);
00148
00149 return img;
00150
00151 }
00152
00153
00154 HalconCpp::HTuple PoseValidation::validateObject(ObjectDescriptor *object, RecognitionResult *recognition_result, Ogre::MeshPtr mesh)
00155 {
00156 bool use_color = object->getModelViewDescriptors().at(recognition_result->getViewIndex()).getUseColor();
00157 HalconCpp::HImage rendered_image = renderObjectImage(recognition_result, mesh);
00158 if (!use_color) {
00159 rendered_image = rendered_image.Rgb1ToGray();
00160 }
00161
00162 HalconCpp::HTuple score;
00163 rendered_image.FindUncalibDescriptorModel(object->getModelViewDescriptors().at(recognition_result->getViewIndex()).getDescModel(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), HalconCpp::HTuple(), object->getModelViewDescriptors().at(recognition_result->getViewIndex()).getScore2D(), 1, "inlier_ratio", &score);
00164
00165 if (score.Length() > 0) {
00166 HalconCpp::HTuple matrix_tuple = object->getModelViewDescriptors().at(recognition_result->getViewIndex()).getDescModel().GetDescriptorModelResults(0, "homography");
00167 return matrix_tuple;
00168 }
00169 return HalconCpp::HTuple();
00170 }
00171
00172
00173 bool PoseValidation::compareHomographyMatrices(HalconCpp::HHomMat2D original, HalconCpp::HHomMat2D rendered, int pose_validation_dist_err)
00174 {
00175 double x_1, y_1, w_1, x_2, y_2, w_2, x_3, y_3, w_3;
00176 x_1 = original.ProjectiveTransPoint2d(0, 0, 1, &y_1, &w_1);
00177 x_2 = original.ProjectiveTransPoint2d(0, 10, 1, &y_2, &w_2);
00178 x_3 = original.ProjectiveTransPoint2d(10, 0, 1, &y_3, &w_3);
00179
00180 double x_rend_1, y_rend_1, w_rend_1, x_rend_2, y_rend_2, w_rend_2, x_rend_3, y_rend_3, w_rend_3;
00181 x_rend_1 = rendered.ProjectiveTransPoint2d(0, 0, 1, &y_rend_1, &w_rend_1);
00182 x_rend_2 = rendered.ProjectiveTransPoint2d(0, 10, 1, &y_rend_2, &w_rend_2);
00183 x_rend_3 = rendered.ProjectiveTransPoint2d(10, 0, 1, &y_rend_3, &w_rend_3);
00184
00185 x_1 = x_1 / w_1;
00186 y_1 = y_1 / w_1;
00187 x_2 = x_2 / w_2;
00188 y_2 = y_2 / w_2;
00189 x_3 = x_3 / w_3;
00190 y_3 = y_3 / w_3;
00191
00192 x_rend_1 = x_rend_1 / w_rend_1;
00193 y_rend_1 = y_rend_1 / w_rend_1;
00194 x_rend_2 = x_rend_2 / w_rend_2;
00195 y_rend_2 = y_rend_2 / w_rend_2;
00196 x_rend_3 = x_rend_3 / w_rend_3;
00197 y_rend_3 = y_rend_3 / w_rend_3;
00198
00199 double dist_1, dist_2, dist_3;
00200 dist_1 = sqrt(pow(x_1 - x_rend_1, 2) + pow(y_1 - y_rend_1, 2));
00201 dist_2 = sqrt(pow(x_2 - x_rend_2, 2) + pow(y_2 - y_rend_2, 2));
00202 dist_3 = sqrt(pow(x_3 - x_rend_3, 2) + pow(y_3 - y_rend_3, 2));
00203
00204 ROS_DEBUG_STREAM("Pose validation: Max. error = " << pose_validation_dist_err << ", dist1: " << dist_1 << ", dist2: " << dist_2 << ", dist3: " << dist_3);
00205 return dist_1 < pose_validation_dist_err && dist_2 < pose_validation_dist_err && dist_3 < pose_validation_dist_err;
00206 }
00207
00208 }