Eigen::Matrix4f parse_registration(turtle_input& input, const std::string& guid_0, const std::string& guid_1) { std::pair<std::string, bool> t = parse_transformation_string(input, guid_0, guid_1); std::istringstream iss(t.first); std::vector<std::string> tokens{std::istream_iterator<std::string>{iss}, std::istream_iterator<std::string>{}}; assert(tokens.size() == 16); Eigen::Matrix4f m; for (uint32_t i = 0; i < 4; ++i) { for (uint32_t j = 0; j < 4; ++j) { double value; std::stringstream convert_str(tokens[i*4+j]); convert_str >> value; convert_str.str(""); m(i, j) = value; } } if (t.second) { Eigen::Matrix4f inv = m.inverse(); m = inv; } return m; }
Eigen::Matrix4f GroundTruthOdometry::getTransformation(uint64_t timestamp) { Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); if(last_utime != 0) { std::map<uint64_t, Eigen::Isometry3f>::const_iterator it = camera_trajectory.find(last_utime); if (it == camera_trajectory.end()) { last_utime = timestamp; return pose; } //Poses are stored in the file in iSAM basis, undo it Eigen::Matrix4f M; M << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; pose = M.inverse() * camera_trajectory[timestamp] * M; } else { std::map<uint64_t, Eigen::Isometry3f>::const_iterator it = camera_trajectory.find(timestamp); Eigen::Isometry3f ident = it->second; pose = Eigen::Matrix4f::Identity(); camera_trajectory[last_utime] = ident; } last_utime = timestamp; return pose; }
void Scene::initScene(CloudPtr &pc_in, CloudPtr &pc_transformed, MatrixXf boundBox, Eigen::Matrix4f eigen_virtualFromFocus_old) { pc_scene.reset (new Cloud); pc_scene_focus.reset (new Cloud); pc_scene_focus_back.reset(new Cloud); for(int i=0;i<pc_in->points.size();i++){ PointT point_in = pc_in->points[i]; PointT point_transformed = pc_transformed->points[i]; // cut off if(point_transformed.x<boundBox(0,0) && point_transformed.x>boundBox(0,1) && point_transformed.y<boundBox(1,0) && point_transformed.y>boundBox(1,1) && point_transformed.z<boundBox(2,0) && point_transformed.z>boundBox(2,1)){ pc_scene->points.push_back(point_in); pc_scene_focus->points.push_back(point_transformed); } } // return back pc_scene to the original cam pos Eigen::Matrix4f inv = eigen_virtualFromFocus_old.inverse(); for(int i=0;i<pc_scene_focus->points.size();i++){ PointT point = pc_scene_focus->points[i]; PointT point_back; point_back.x = inv(0,0)*point.x + inv(0,1)*point.y + inv(0,2)*point.z + inv(0,3); point_back.y = inv(1,0)*point.x + inv(1,1)*point.y + inv(1,2)*point.z + inv(1,3); point_back.z = inv(2,0)*point.x + inv(2,1)*point.y + inv(2,2)*point.z + inv(2,3); pc_scene_focus_back->points.push_back(point_back); } // if(USENORM) calcNormal(); }
void Communication::publish_objectToGrasp_moveitFormat(){ ros::Rate r(1); ros::NodeHandle node; static tf::TransformBroadcaster br; Eigen::Matrix4f camToWorldMatrix; camToWorldMatrix.setZero();camToWorldMatrix(0,2) = 1; camToWorldMatrix(1,0) = -1; camToWorldMatrix(2,1) = -1; camToWorldMatrix(3,3) = 1; while(node.ok()){ pcl::PointCloud<PointT>::Ptr object_pc = m_object_ex_ptr->getObjectToGrasp(); pcl::PointCloud<PointT>::Ptr object_pc_transformed(new pcl::PointCloud<PointT>); // Eigen::Vector4f c = m_object_ex_ptr->getGraspCentroid(); c(3)=1; // tf::Transform simpleTF; tf::StampedTransform camToJacoTf; tf::TransformListener listener; bool tf_ready = listener.waitForTransform("camera_rgb_frame","root",ros::Time(0),ros::Duration(5.0)); if(object_pc->size() > 0 && tf_ready){ listener.lookupTransform("camera_rgb_frame","root",ros::Time(0),camToJacoTf); Eigen::Matrix4f camToJacoMatrix; pcl_ros::transformAsMatrix(camToJacoTf,camToJacoMatrix); Eigen::Matrix4f combinedMatrix = camToJacoMatrix.inverse() * camToWorldMatrix; // Eigen::Vector4f result = combinedMatrix * c; // Eigen::Matrix4f res; res(0,3) = result(0); res(1,3) = result(1); res(2,3) = result(2); res(3,3) = 1; // simpleTF = tfFromEigen(res); // br.sendTransform(tf::StampedTransform(simpleTF, ros::Time::now(), "root", "object_centroid_test")); pcl::transformPointCloud(*object_pc, *object_pc_transformed, combinedMatrix); // std::cout << "old : " << object_pc->at(100) << std::endl; // std::cout << "new : " << object_pc_transformed->at(100) << std::endl; ObjectToGrasp_publisher_.publish(object_pc_transformed); } r.sleep(); } }
void IndexMap::predictIndices(const Eigen::Matrix4f & pose, const int & time, const std::pair<GLuint, GLuint> & model, const float depthCutoff, const int timeDelta) { indexFrameBuffer.Bind(); glPushAttrib(GL_VIEWPORT_BIT); glViewport(0, 0, indexRenderBuffer.width, indexRenderBuffer.height); glClearColor(0, 0, 0, 0); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); indexProgram->Bind(); Eigen::Matrix4f t_inv = pose.inverse(); Eigen::Vector4f cam(Intrinsics::getInstance().cx() * IndexMap::FACTOR, Intrinsics::getInstance().cy() * IndexMap::FACTOR, Intrinsics::getInstance().fx() * IndexMap::FACTOR, Intrinsics::getInstance().fy() * IndexMap::FACTOR); indexProgram->setUniform(Uniform("t_inv", t_inv)); indexProgram->setUniform(Uniform("cam", cam)); indexProgram->setUniform(Uniform("maxDepth", depthCutoff)); indexProgram->setUniform(Uniform("cols", (float)Resolution::getInstance().cols() * IndexMap::FACTOR)); indexProgram->setUniform(Uniform("rows", (float)Resolution::getInstance().rows() * IndexMap::FACTOR)); indexProgram->setUniform(Uniform("time", time)); indexProgram->setUniform(Uniform("timeDelta", timeDelta)); glBindBuffer(GL_ARRAY_BUFFER, model.first); glEnableVertexAttribArray(0); glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, 0); glEnableVertexAttribArray(1); glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f))); glEnableVertexAttribArray(2); glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f) * 2)); glDrawTransformFeedback(GL_POINTS, model.second); glDisableVertexAttribArray(0); glDisableVertexAttribArray(1); glDisableVertexAttribArray(2); glBindBuffer(GL_ARRAY_BUFFER, 0); indexFrameBuffer.Unbind(); indexProgram->Unbind(); glPopAttrib(); glFinish(); }
bool isOdometryContinuousMotion(Eigen::Matrix4f &prevPose, Eigen::Matrix4f &currPose, float thresDist = 0.1) { Eigen::Matrix4f relativePose = prevPose.inverse() * currPose; if( relativePose.block(0,3,3,1).norm() > thresDist ) return false; return true; }
void Scene::GetCameraRay(double x, double y, Eigen::Vector3d* origin, Eigen::Vector3d* ray) { Eigen::Matrix4f inverse; inverse = g_viewMatrix.inverse(); Eigen::Vector4f preVec; preVec << (2 * x / DDWIDTH) - 1, 1 - (2 * y / DDHEIGHT), 2 * .5 - 1, 1; (*origin)[0] = xpos; (*origin)[1] = ypos; (*origin)[2] = zpos; Eigen::Vector4f ori = inverse * preVec; (*ray)[0] = ori[0] - (*origin)[0]; (*ray)[1] = ori[1] - (*origin)[1]; (*ray)[2] = ori[2] - (*origin)[2]; }
void DynamicsRobot::setGlobalPose(Eigen::Matrix4f &gp) { Eigen::Matrix4f currentPose = robot->getGlobalPose(); Eigen::Matrix4f delta = gp * currentPose.inverse(); robot->setGlobalPose(gp); std::map<VirtualRobot::RobotNodePtr, DynamicsObjectPtr>::iterator it = dynamicRobotNodes.begin(); while (it != dynamicRobotNodes.end()) { Eigen::Matrix4f newPose = it->second->getSceneObject()->getGlobalPose() * delta; it->second->setPose(newPose); it++; } }
void DepthFrame::getRegisteredAndReferencedPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud) { pcl::PointXYZ regReferencePoint = getRegisteredReferencePoint(); // Build a translation matrix to the registered reference the cloud after its own registration Eigen::Matrix4f E = Eigen::Matrix4f::Identity(); E.col(3) = regReferencePoint.getVector4fMap(); // set translation column // Apply registration first and then referenciation (right to left order in matrix product) pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudTmp (new pcl::PointCloud<pcl::PointXYZ>); getPointCloud(*pCloudTmp); pcl::transformPointCloud(*pCloudTmp, cloud, E.inverse() * m_T); }
void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud, double _dim[3], double _trans[3], double _rot[3] ) { // 1. Compute the bounding box center Eigen::Vector4d centroid; pcl::compute3DCentroid( *_cloud, centroid ); _trans[0] = centroid(0); _trans[1] = centroid(1); _trans[2] = centroid(2); // 2. Compute main axis orientations pcl::PCA<PointT> pca; pca.setInputCloud( _cloud ); Eigen::Vector3f eigVal = pca.getEigenValues(); Eigen::Matrix3f eigVec = pca.getEigenVectors(); // Make sure 3 vectors are normal w.r.t. each other Eigen::Vector3f temp; eigVec.col(2) = eigVec.col(0); // Z Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) ); eigVec.col(0) = v3; Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0); _rot[0] = (double)rpy(2); _rot[1] = (double)rpy(1); _rot[2] = (double)rpy(0); // Transform _cloud Eigen::Matrix4f transf = Eigen::Matrix4f::Identity(); transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2); transf.block(0,0,3,3) = eigVec; Eigen::Matrix4f tinv; tinv = transf.inverse(); PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() ); pcl::transformPointCloud( *_cloud, *cloud_temp, tinv ); // Get maximum and minimum PointT minPt; PointT maxPt; pcl::getMinMax3D( *cloud_temp, minPt, maxPt ); _dim[0] = ( maxPt.x - minPt.x ) / 2.0; _dim[1] = ( maxPt.y - minPt.y ) / 2.0; _dim[2] = ( maxPt.z - minPt.z ) / 2.0; }
void Camera::applyViewMatrix(MatrixStack *MV) const { // Create the translation and rotation matrices Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); T(0,3) = position(0); T(1,3) = position(1); T(2,3) = position(2); Eigen::Matrix4f YR = Eigen::Matrix4f::Identity(); YR.block<3,3>(0,0) = Eigen::AngleAxisf(yawRotation, Eigen::Vector3f(0.0f, 1.0f, 0.0f)).toRotationMatrix(); Eigen::Matrix4f PR = Eigen::Matrix4f::Identity(); PR.block<3,3>(0,0) = Eigen::AngleAxisf(pitchRotation, Eigen::Vector3f(1.0f, 0.0f, 0.0f)).toRotationMatrix(); // The matrix C is the product of these matrices Eigen::Matrix4f C = T * YR * PR; // Also apply rotations here // The view matrix is the inverse Eigen::Matrix4f V = C.inverse(); // Add to the matrix stack MV->multMatrix(V); }
void Scene::focusSceneUpdate(CloudPtr &pc_in, tf::Transform &transform_camFromBase, tf::Transform &transform_baseFromfocus, MatrixXf boundBox, Eigen::Matrix4f eigen_virtualFromFocus_old) { pc_scene.reset (new Cloud); pc_scene_focus.reset (new Cloud); pc_scene_base.reset(new Cloud); pc_scene_focus_back.reset(new Cloud); CloudPtr pc_scene_focus_temp; pc_scene_focus_temp.reset (new Cloud); Eigen::Matrix4f eigen_camFromBase; transformAsMatrix (transform_camFromBase, eigen_camFromBase); if(DEBUG_ALGORITHM) cout<<"-------------camupdated"<<endl; if(DEBUG_ALGORITHM) cout<<eigen_camFromBase<<endl; pcl_ros::transformPointCloud(*pc_in, *pc_scene_base, transform_camFromBase); pcl_ros::transformPointCloud(*pc_scene_base, *pc_scene_focus_temp, transform_baseFromfocus); for(int i=0;i<pc_in->points.size();i++){ PointT point_in = pc_in->points[i]; PointT point_transformed = pc_scene_focus_temp->points[i]; // cut off if(point_transformed.x<boundBox(0,0) && point_transformed.x>boundBox(0,1) && point_transformed.y<boundBox(1,0) && point_transformed.y>boundBox(1,1) && point_transformed.z<boundBox(2,0) && point_transformed.z>boundBox(2,1)){ pc_scene->points.push_back(point_in); pc_scene_focus->points.push_back(point_transformed); } } // return back pc_scene to the original cam pos Eigen::Matrix4f inv = eigen_virtualFromFocus_old.inverse(); for(int i=0;i<pc_scene_focus->points.size();i++){ PointT point = pc_scene_focus->points[i]; PointT point_back; point_back.x = inv(0,0)*point.x + inv(0,1)*point.y + inv(0,2)*point.z + inv(0,3); point_back.y = inv(1,0)*point.x + inv(1,1)*point.y + inv(1,2)*point.z + inv(1,3); point_back.z = inv(2,0)*point.x + inv(2,1)*point.y + inv(2,2)*point.z + inv(2,3); pc_scene_focus_back->points.push_back(point_back); } // if(USENORM) calcNormal(); }
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_get_reg_cloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud) { pcl::PointCloud<pcl::PointXYZ>::Ptr reduced_source, reduced_target; reduced_source = point_cloud_xyzrgb_to_xyz(source_cloud); //Here, we create new cloud. So you can handle reduced clouds regardless of orignial ones reduced_target = point_cloud_xyzrgb_to_xyz(target_cloud); reduced_source = point_prepare_icp(reduced_source); reduced_target = point_prepare_icp(reduced_target); //Prepare clouds to be icp-ed Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); Eigen::Matrix4f target_to_source_transformation; //Transformation Matrix pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //ICP Filter pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result (new pcl::PointCloud<pcl::PointXYZ>); std::vector<unsigned int> score; for(int i = 0; i < reg_iteration ; i ++) { icp.setInputCloud(reduced_source); icp.setInputTarget(reduced_target); //set ICP icp.align(*reduced_source); transform *= icp.getFinalTransformation(); unsigned int current_score = (unsigned int) icp.getFitnessScore(); if(i>1 && score.back()-current_score < 50) { std::cout<<"ran"<<i<<"times, with score : "<<icp.getFitnessScore() << std::endl; break; } else score.push_back(current_score); } target_to_source_transformation = transform.inverse(); //"We got transformation matrix, now transforming target into source's frame" pcl::transformPointCloud(*target_cloud, *target_cloud, target_to_source_transformation); //"Transformation done. Returning transformed cloud" *source_cloud += *target_cloud; //Merge them return source_cloud; }
Eigen::Matrix4f computeGroundTruth(const std::string path_groundtruth, const int id) { std::string line; std::ifstream file(path_groundtruth); int offset = 3; std::vector<std::string> splitVec; int index = id * 5 + offset; Eigen::Matrix4f transformationMatrix; if (file.is_open()) { int count = 1; bool done = false; while (count<index) { std::getline(file, line); count++; } //start reading matrix from file for (int i = 0; i<4; i++) { std::getline(file, line); boost::split(splitVec, line, boost::is_any_of("\t"), boost::token_compress_on); transformationMatrix.row(i) << boost::lexical_cast<float>(splitVec.at(0)), boost::lexical_cast<float>(splitVec.at(1)), boost::lexical_cast<float>(splitVec.at(2)), boost::lexical_cast<float>(splitVec.at(3)); } } else std::cout << "GroundTruth file not found" << std::endl; transformationMatrix.inverse(); return transformationMatrix; }
void pcl::ihs::OfflineIntegration::computationThread () { std::vector <std::string> filenames; if (!this->getFilesFromDirectory (path_dir_, ".pcd", filenames)) { std::cerr << "ERROR in offline_integration.cpp: Could not get the files from the directory\n"; return; } // First cloud is reference model std::cerr << "Processing file " << std::setw (5) << 1 << " / " << filenames.size () << std::endl; CloudXYZRGBNormalPtr cloud_model (new CloudXYZRGBNormal ()); Eigen::Matrix4f T = Eigen::Matrix4f::Identity (); if (!this->load (filenames [0], cloud_model, T)) { std::cerr << "ERROR in offline_integration.cpp: Could not load the model cloud.\n"; return; } pcl::transformPointCloudWithNormals (*cloud_model, *cloud_model, T); if (!integration_->reconstructMesh (cloud_model, mesh_model_)) { std::cerr << "ERROR in offline_integration.cpp: Could not reconstruct the model mesh.\n"; return; } Base::setPivot ("model"); Base::addMesh (mesh_model_, "model"); if (filenames.size () < 1) { return; } for (unsigned int i=1; i<filenames.size (); ++i) { std::cerr << "Processing file " << std::setw (5) << i+1 << " / " << filenames.size () << std::endl; { boost::mutex::scoped_lock lock (mutex_); if (destructor_called_) return; } boost::mutex::scoped_lock lock_quit (mutex_quit_); CloudXYZRGBNormalPtr cloud_data (new CloudXYZRGBNormal ()); if (!this->load (filenames [i], cloud_data, T)) { std::cerr << "ERROR in offline_integration.cpp: Could not load the data cloud.\n"; return; } if (!integration_->merge (cloud_data, mesh_model_, T)) { std::cerr << "ERROR in offline_integration.cpp: merge failed.\n"; return; } integration_->age (mesh_model_); { lock_quit.unlock (); boost::mutex::scoped_lock lock (mutex_); if (destructor_called_) return; Base::addMesh (mesh_model_, "model", Eigen::Isometry3d (T.inverse ().cast <double> ())); Base::calcFPS (computation_fps_); } } Base::setPivot ("model"); }
void RegisteredViewsSource<Full3DPointT, PointInT, OutModelPointT>::loadModel (ModelT & model) { const std::string training_view_path = path_ + model.class_ + "/" + model.id_ + "/views/"; const std::string view_pattern = ".*" + view_prefix_ + ".*.pcd"; model.view_filenames_ = io::getFilesInDirectory(training_view_path, view_pattern, false); std::cout << "Object class: " << model.class_ << ", id: " << model.id_ << ", views: " << model.view_filenames_.size() << std::endl; typename pcl::PointCloud<Full3DPointT>::Ptr modell (new pcl::PointCloud<Full3DPointT>); typename pcl::PointCloud<Full3DPointT>::Ptr modell_voxelized (new pcl::PointCloud<Full3DPointT>); pcl::io::loadPCDFile(path_ + model.class_ + "/" + model.id_ + "/3D_model.pcd", *modell); float voxel_grid_size = 0.003f; typename pcl::VoxelGrid<Full3DPointT> grid_; grid_.setInputCloud (modell); grid_.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size); grid_.setDownsampleAllData (true); grid_.filter (*modell_voxelized); model.normals_assembled_.reset(new pcl::PointCloud<pcl::Normal>); model.assembled_.reset (new pcl::PointCloud<PointInT>); pcl::copyPointCloud(*modell_voxelized, *model.assembled_); pcl::copyPointCloud(*modell_voxelized, *model.normals_assembled_); if(load_into_memory_) { model.views_.resize( model.view_filenames_.size() ); model.indices_.resize( model.view_filenames_.size() ); model.poses_.resize( model.view_filenames_.size() ); model.self_occlusions_.resize( model.view_filenames_.size() ); for (size_t i=0; i<model.view_filenames_.size(); i++) { // load training view const std::string view_file = training_view_path + "/" + model.view_filenames_[i]; model.views_[i].reset( new pcl::PointCloud<PointInT> () ); pcl::io::loadPCDFile (view_file, *model.views_[i]); // read pose std::string pose_fn (view_file); boost::replace_last (pose_fn, view_prefix_, pose_prefix_); boost::replace_last (pose_fn, ".pcd", ".txt"); Eigen::Matrix4f pose = io::readMatrixFromFile( pose_fn ); model.poses_[i] = pose.inverse(); //the recognizer assumes transformation from M to CC - i think! // read object mask model.indices_[i].indices.clear(); std::string obj_indices_fn (view_file); boost::replace_last (obj_indices_fn, view_prefix_, indices_prefix_); boost::replace_last (obj_indices_fn, ".pcd", ".txt"); std::ifstream f ( obj_indices_fn.c_str() ); int idx; while (f >> idx) model.indices_[i].indices.push_back(idx); f.close(); model.self_occlusions_[i] = -1.f; } } else {
/** Convert transform in global (ladybug) coordinates to local (camera) coordinates. * @param Eigen::Matrix4f transform in global coordinates * @param int index of the previous camera * @param int index of the current camera * @return Eigen::Matrix4f transform in local coordinates */ Eigen::Matrix4f Ladybug2::Ladybug2CamRef(Eigen::Matrix4f TGlobal, int camNoPrev, int camNoCurr) { Eigen::Matrix4f TLocalInv = extrinsics_[camNoPrev].inverse() * TGlobal * extrinsics_[camNoCurr]; Eigen::Matrix4f TLocal = TLocalInv.inverse(); return TLocal; }
/** Convert transform in local (camera) coordinates to global (ladybug) coordinates. * @param Eigen::Matrix4f transform in local coordinates * @param int index of the previous camera * @param int index of the current camera * @return Eigen::Matrix4f transform in global coordinates */ Eigen::Matrix4f Ladybug2::cam2LadybugRef(Eigen::Matrix4f TLocal, int camNoPrev, int camNoCurr) { Eigen::Matrix4f TGlobal = extrinsics_[camNoPrev] * TLocal.inverse() * extrinsics_[camNoCurr].inverse(); return TGlobal; }
/* * 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"); }
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft, const Eigen::Vector3f& localAnkleRight, const Eigen::Matrix4f& leftFootPoseGroundFrame, const Eigen::Matrix4f& rightFootPoseGroundFrame, const Eigen::Vector3f& zmpRefGroundFrame, Bipedal::SupportPhase phase) { Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame); Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame); Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse()); Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse()); double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase); //std::cout << "########## " << alpha << " ###########" << std::endl; ForceTorque ft; // kg*m/s^2 = N ft.leftForce = -(1-alpha) * mass * gravity; ft.rightForce = -alpha * mass * gravity; // Note we need force as kg*mm/s^2 ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000); ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000); // ZMP not contained in convex polygone if (std::fabs(alpha) > std::numeric_limits<float>::epsilon() && std::fabs(alpha-1) > std::numeric_limits<float>::epsilon()) { Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque; Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque; Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame); //std::cout << "Tau0World: " << tau0.transpose() << std::endl; //std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl; //std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl; // Note: Our coordinate system is different than in the paper! // Also this is not the same as the ground frame. Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft - localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1); xAxis /= xAxis.norm(); Eigen::Vector3f zAxis(0, 0, 1); Eigen::Vector3f yAxis = zAxis.cross(xAxis); yAxis /= yAxis.norm(); Eigen::Matrix3f centerFrame; centerFrame.block(0, 0, 3, 1) = xAxis; centerFrame.block(0, 1, 3, 1) = yAxis; centerFrame.block(0, 2, 3, 1) = zAxis; //std::cout << "Center frame:\n" << centerFrame << std::endl; Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0; Eigen::Vector3f leftTorqueCenter; leftTorqueCenter.x() = (1-alpha)*centerTau0.x(); leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0; leftTorqueCenter.z() = 0; Eigen::Vector3f rightTorqueCenter; rightTorqueCenter.x() = alpha*centerTau0.x(); rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y(); rightTorqueCenter.z() = 0; //std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl; //std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl; //std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl; // tcp <--- ground frame <--- center frame ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter; ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter; } // Torque depends on timestep, we need a way to do this correctly. const double torqueFactor = 1; // convert to Nm ft.leftTorque *= torqueFactor / 1000.0 / 1000.0; ft.rightTorque *= torqueFactor / 1000.0 / 1000.0; //std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl; //std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl; return ft; }
int volumetric_knt_cuda(int argc, char **argv) { Timer timer; int vol_size = vx_count * vx_size; float half_vol_size = vol_size * 0.5f; Eigen::Vector3i voxel_size(vx_size, vx_size, vx_size); Eigen::Vector3i volume_size(vol_size, vol_size, vol_size); Eigen::Vector3i voxel_count(vx_count, vx_count, vx_count); int total_voxels = voxel_count.x() * voxel_count.y() * voxel_count.z(); std::cout << std::fixed << "Voxel Count : " << voxel_count.transpose() << std::endl << "Voxel Size : " << voxel_size.transpose() << std::endl << "Volume Size : " << volume_size.transpose() << std::endl << "Total Voxels : " << total_voxels << std::endl << std::endl; timer.start(); KinectFrame knt(filepath); timer.print_interval("Importing knt frame : "); Eigen::Affine3f grid_affine = Eigen::Affine3f::Identity(); grid_affine.translate(Eigen::Vector3f(0, 0, half_vol_size)); grid_affine.scale(Eigen::Vector3f(1, 1, -1)); // z is negative inside of screen Eigen::Matrix4f grid_matrix = grid_affine.matrix(); float knt_near_plane = 0.1f; float knt_far_plane = 10240.0f; Eigen::Matrix4f projection = perspective_matrix<float>(KINECT_V2_FOVY, KINECT_V2_DEPTH_ASPECT_RATIO, knt_near_plane, knt_far_plane); Eigen::Matrix4f projection_inverse = projection.inverse(); Eigen::Matrix4f view_matrix = Eigen::Matrix4f::Identity(); std::vector<float4> vertices(knt.depth.size(), make_float4(0, 0, 0, 1)); std::vector<float4> normals(knt.depth.size(), make_float4(0, 0, 1, 1)); std::vector<Eigen::Vector2f> grid_voxels_params(total_voxels); // // setup image parameters // unsigned short image_width = 2; unsigned short image_height = 2; uchar4* image_data = new uchar4[image_width * image_height]; memset(image_data, 0, image_width * image_height * sizeof(uchar4)); float4* debug_buffer = new float4[image_width * image_height]; memset(debug_buffer, 0, image_width * image_height * sizeof(float4)); knt_cuda_setup( vx_count, vx_size, grid_matrix.data(), projection.data(), projection_inverse.data(), *grid_voxels_params.data()->data(), KINECT_V2_DEPTH_WIDTH, KINECT_V2_DEPTH_HEIGHT, KINECT_V2_DEPTH_MIN, KINECT_V2_DEPTH_MAX, vertices.data()[0], normals.data()[0], image_width, image_height, *image_data, *debug_buffer ); timer.start(); knt_cuda_allocate(); knt_cuda_init_grid(); timer.print_interval("Allocating gpu : "); timer.start(); knt_cuda_copy_host_to_device(); knt_cuda_copy_depth_buffer_to_device(knt.depth.data()); timer.print_interval("Copy host to device : "); timer.start(); knt_cuda_normal_estimation(); timer.print_interval("Normal estimation : "); timer.start(); knt_cuda_update_grid(view_matrix.data()); timer.print_interval("Update grid : "); timer.start(); knt_cuda_grid_params_copy_device_to_host(); knt_cuda_copy_device_to_host(); timer.print_interval("Copy device to host : "); timer.start(); knt_cuda_free(); timer.print_interval("Cleanup gpu : "); timer.start(); Eigen::Affine3f grid_affine_2 = Eigen::Affine3f::Identity(); grid_affine_2.translate(Eigen::Vector3f(-half_vol_size, -half_vol_size, -vol_size)); export_volume( "../../data/grid_volume_gpu_knt.obj", voxel_count, voxel_size, grid_voxels_params); //grid_affine_2.matrix()); export_obj_with_colors("../../data/knt_grid_frame_normals.obj", vertices, normals); timer.print_interval("Exporting volume : "); return 0; }
void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) { PointCloud::Ptr src(new PointCloud); PointCloud::Ptr tgt(new PointCloud); pcl::VoxelGrid<PointType> grid; if(downsample) { grid.setLeafSize(0.05, 0.05, 0.05); grid.setInputCloud(cloud_src); grid.filter(*src); grid.setInputCloud(cloud_tgt); grid.filter(*tgt); } else { src = cloud_src; tgt = cloud_tgt; } PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals); PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals); pcl::NormalEstimation<PointType, PointNormalT> norm_est; pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType> ()); norm_est.setSearchMethod(tree); norm_est.setKSearch(30); norm_est.setInputCloud(src); norm_est.compute(*points_with_normals_src); pcl::copyPointCloud(*src, *points_with_normals_src); norm_est.setInputCloud(tgt); norm_est.compute(*points_with_normals_tgt); pcl::copyPointCloud(*tgt, *points_with_normals_tgt); PointRep point_representation; float alpha[4] = {1.0, 1.0, 1.0, 1.0}; point_representation.setRescaleValues(alpha); pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; reg.setTransformationEpsilon(1e-5); reg.setMaxCorrespondenceDistance(0.15); reg.setPointRepresentation(boost::make_shared<const PointRep> (point_representation)); reg.setInputCloud(points_with_normals_src); reg.setInputTarget(points_with_normals_tgt); //Optimize Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource; PointCloudWithNormals::Ptr reg_result = points_with_normals_src; reg.setMaximumIterations(2); for(int i = 0; i < 300; ++i) { points_with_normals_src = reg_result; reg.setInputCloud(points_with_normals_src); reg.align(*reg_result); Ti = reg.getFinalTransformation() * Ti; if(fabs ((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon()) reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001); prev = reg.getLastIncrementalTransformation(); //showCloudsRight(points_with_normals_tgt, points_with_normals_src); std::cout << "Fitness: " << reg.getFitnessScore() << std::endl; } targetToSource = Ti.inverse(); pcl::transformPointCloud(*cloud_tgt, *output, targetToSource); p->removePointCloud ("source"); p->removePointCloud ("target"); PointCloudColorHandlerCustom<PointType> cloud_tgt_h (output, 0, 255, 0); PointCloudColorHandlerCustom<PointType> cloud_src_h (cloud_src, 255, 0, 0); p->addPointCloud (output, cloud_tgt_h, "target", vp_2); p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2); PCL_INFO ("Press q to continue the registration.\n"); p->spinOnce (); *output += *cloud_src; final_transform = targetToSource; }
void GlobalModel::clean(const Eigen::Matrix4f & pose, const int & time, GPUTexture * indexMap, GPUTexture * vertConfMap, GPUTexture * colorTimeMap, GPUTexture * normRadMap, GPUTexture * depthMap, const float confThreshold, std::vector<float> & graph, const int timeDelta, const float maxDepth, const bool isFern) { assert(graph.size() / 16 < MAX_NODES); if(graph.size() > 0) { //Can be optimised by only uploading new nodes with offset glBindTexture(GL_TEXTURE_2D, deformationNodes.texture->tid); glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, graph.size(), 1, GL_LUMINANCE, GL_FLOAT, graph.data()); } TICK("Fuse::Copy"); //Next we copy the new unstable vertices from the newUnstableFid transform feedback into the global map unstableProgram->Bind(); unstableProgram->setUniform(Uniform("time", time)); unstableProgram->setUniform(Uniform("confThreshold", confThreshold)); unstableProgram->setUniform(Uniform("scale", (float)IndexMap::FACTOR)); unstableProgram->setUniform(Uniform("indexSampler", 0)); unstableProgram->setUniform(Uniform("vertConfSampler", 1)); unstableProgram->setUniform(Uniform("colorTimeSampler", 2)); unstableProgram->setUniform(Uniform("normRadSampler", 3)); unstableProgram->setUniform(Uniform("nodeSampler", 4)); unstableProgram->setUniform(Uniform("depthSampler", 5)); unstableProgram->setUniform(Uniform("nodes", (float)(graph.size() / 16))); unstableProgram->setUniform(Uniform("nodeCols", (float)NODE_TEXTURE_DIMENSION)); unstableProgram->setUniform(Uniform("timeDelta", timeDelta)); unstableProgram->setUniform(Uniform("maxDepth", maxDepth)); unstableProgram->setUniform(Uniform("isFern", (int)isFern)); Eigen::Matrix4f t_inv = pose.inverse(); unstableProgram->setUniform(Uniform("t_inv", t_inv)); unstableProgram->setUniform(Uniform("cam", Eigen::Vector4f(Intrinsics::getInstance().cx(), Intrinsics::getInstance().cy(), Intrinsics::getInstance().fx(), Intrinsics::getInstance().fy()))); unstableProgram->setUniform(Uniform("cols", (float)Resolution::getInstance().cols())); unstableProgram->setUniform(Uniform("rows", (float)Resolution::getInstance().rows())); glBindBuffer(GL_ARRAY_BUFFER, vbos[target].first); glEnableVertexAttribArray(0); glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, 0); glEnableVertexAttribArray(1); glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f))); glEnableVertexAttribArray(2); glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f) * 2)); glEnable(GL_RASTERIZER_DISCARD); glBindTransformFeedback(GL_TRANSFORM_FEEDBACK, vbos[renderSource].second); glBindBufferBase(GL_TRANSFORM_FEEDBACK_BUFFER, 0, vbos[renderSource].first); glBeginTransformFeedback(GL_POINTS); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, indexMap->texture->tid); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, vertConfMap->texture->tid); glActiveTexture(GL_TEXTURE2); glBindTexture(GL_TEXTURE_2D, colorTimeMap->texture->tid); glActiveTexture(GL_TEXTURE3); glBindTexture(GL_TEXTURE_2D, normRadMap->texture->tid); glActiveTexture(GL_TEXTURE4); glBindTexture(GL_TEXTURE_2D, deformationNodes.texture->tid); glActiveTexture(GL_TEXTURE5); glBindTexture(GL_TEXTURE_2D, depthMap->texture->tid); glBeginQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN, countQuery); glDrawTransformFeedback(GL_POINTS, vbos[target].second); glBindBuffer(GL_ARRAY_BUFFER, newUnstableVbo); glEnableVertexAttribArray(0); glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, 0); glEnableVertexAttribArray(1); glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f))); glEnableVertexAttribArray(2); glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, Vertex::SIZE, reinterpret_cast<GLvoid*>(sizeof(Eigen::Vector4f) * 2)); glDrawTransformFeedback(GL_POINTS, newUnstableFid); glEndQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN); glGetQueryObjectuiv(countQuery, GL_QUERY_RESULT, &count); glEndTransformFeedback(); glDisable(GL_RASTERIZER_DISCARD); glBindTexture(GL_TEXTURE_2D, 0); glActiveTexture(GL_TEXTURE0); glDisableVertexAttribArray(0); glDisableVertexAttribArray(1); glDisableVertexAttribArray(2); glBindBuffer(GL_ARRAY_BUFFER, 0); glBindTransformFeedback(GL_TRANSFORM_FEEDBACK, 0); unstableProgram->Unbind(); std::swap(target, renderSource); glFinish(); TOCK("Fuse::Copy"); }
int main (int argc, char** argv) { PointCloudPtr cloud_source (new PointCloud); PointCloudPtr cloud_target (new PointCloud); PointCloudPtr featureCloudSourceTemp (new PointCloud); PointCloudPtr featureCloudTargetTemp (new PointCloud); PointCloudPtr cloud_converg_sparse_all (new PointCloud); PointCloudPtr cloud_converg_sparse_correspond (new PointCloud); PointCloudPtr cloud_target_sparse_correspond (new PointCloud); PointCloudPtr cloud_converg_dense (new PointCloud); PointCloudPtr cloud_ransac_estimation (new PointCloud); PointCloudNormalPtr cloud_source_normals (new PointCloudNormal); PointCloudNormalPtr cloud_target_normals (new PointCloudNormal); PointCloudNormalPtr featureCloudSource (new PointCloudNormal); PointCloudNormalPtr featureCloudTarget (new PointCloudNormal); Eigen::Matrix4f initialTransform; std::vector<int> indicesSource; std::vector<int> indicesTarget; //Fill in the cloud data //pcl::PCDReader reader; //ROS_INFO("Reading saved clouds with normals from file (faster)"); //reader.read ("normals-source.pcd", *cloud_source_normals); //reader.read ("normals-target.pcd", *cloud_target_normals); //std::cout << "PointCloud source has: " << cloud_source->points.size () << " data points." << std::endl; //std::cout << "PointCloud target has: " << cloud_target->points.size () << " data points." << std::endl; ROS_INFO("Getting test data from a bag file"); getTestDataFromBag(cloud_source, cloud_target, featureCloudSourceTemp, indicesSource, featureCloudTargetTemp, indicesTarget, initialTransform, 0); Eigen::Matrix4f ransacInverse = initialTransform.inverse(); /* // remove corresponances with large z values (susceptible to error) ROS_INFO("Removing feature correspondances with a large depth measurement"); //removeCorrespondancesZThreshold (featureCloudSourceTemp, indicesSource, featureCloudTargetTemp, indicesTarget, 1.3); ROS_INFO_STREAM("indices source size: " << indicesSource.size() << " indices target size: " << indicesTarget.size()); //calculate normals ROS_INFO("Calcualting normals"); normalEstimation(cloud_source, cloud_source_normals); normalEstimation(cloud_target, cloud_target_normals); ROS_INFO("Converting feature point clouds"); pcl::copyPointCloud (*featureCloudSourceTemp, *featureCloudSource); pcl::copyPointCloud (*featureCloudTargetTemp, *featureCloudTarget); // here is a guess transform that was manually set to align point clouds, pure icp performs well with this PointCloud Final; Eigen::Matrix4f guess; guess << 1, 0, 0, 0.07, 0, 1, 0, 0, 0, 0, 1, 0.015, 0, 0, 0, 1; ROS_INFO("Setting up icp with features"); */ /* custom icp pcl::IterativeClosestPointFeatures<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icp_features; icp_features.setMaximumIterations (40); icp_features.setTransformationEpsilon (1e-9); icp_features.setMaxCorrespondenceDistance(0.1); icp_features.setRANSACOutlierRejectionThreshold(0.03); icp_features.setInputCloud(cloud_source_normals); icp_features.setInputTarget(cloud_target_normals); icp_features.setSourceFeatures (featureCloudSource, indicesSource); icp_features.setTargetFeatures (featureCloudTarget, indicesTarget); icp_features.setFeatureErrorWeight(0.3); // 1 = feature, 0 = icp ROS_INFO("Performing rgbd icp....."); icp_features.align(Final, ransacInverse); std::cout << "ICP features has finished with converge flag of:" << icp_features.hasConverged() << " score: " << icp_features.getFitnessScore() << std::endl; std::cout << icp_features.getFinalTransformation() << std::endl; */ /* std::vector<int> indicesSourceSmall = indicesSource; std::vector<int> indicesTargetSmall = indicesTarget; for(std::vector<int>::iterator iterator_ = indicesSource.begin(); iterator_ != indicesSource.end(); ++iterator_) { *iterator_ = *iterator_ + cloud_source->size(); } for(std::vector<int>::iterator iterator_ = indicesTarget.begin(); iterator_ != indicesTarget.end(); ++iterator_) { *iterator_ = *iterator_ + cloud_target->size(); } PointCloudNormalPtr concatinatedSourceCloud (new PointCloudNormal); PointCloudNormalPtr concatinatedTargetCloud (new PointCloudNormal); *concatinatedSourceCloud = *cloud_source_normals; *concatinatedTargetCloud = *cloud_target_normals; (*concatinatedSourceCloud) += *featureCloudSource; (*concatinatedTargetCloud) += *featureCloudTarget; boost::shared_ptr< TransformationEstimationWDF<pcl::PointXYZRGBNormal,pcl::PointXYZRGBNormal> > initialTransformWDF(new TransformationEstimationWDF<pcl::PointXYZRGBNormal,pcl::PointXYZRGBNormal>()); float alpha = 1.0; initialTransformWDF->setAlpha(alpha); initialTransformWDF->setCorrespondecesDFP(indicesSource, indicesTarget); // Instantiate ICP pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icp_wdf; // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored) icp_wdf.setMaxCorrespondenceDistance (0.05); // Set the maximum number of iterations (criterion 1) icp_wdf.setMaximumIterations (75); // Set the transformation epsilon (criterion 2) icp_wdf.setTransformationEpsilon (1e-8); // Set the euclidean distance difference epsilon (criterion 3) icp_wdf.setEuclideanFitnessEpsilon (0); //1 // Set TransformationEstimationWDF as ICP transform estimator icp_wdf.setTransformationEstimation (initialTransformWDF); icp_wdf.setInputCloud( concatinatedSourceCloud); icp_wdf.setInputTarget( concatinatedTargetCloud); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_transformed( new pcl::PointCloud<pcl::PointXYZRGBNormal>); // As before, due to my initial bad naming, it is the "target" that is being transformed // set initial transform icp_wdf.align ( *cloud_transformed, ransacInverse); //init_tr ); std::cout << "[SIIMCloudMatch::runICPMatch] Has converged? = " << icp_wdf.hasConverged() << std::endl << " fitness score (SSD): " << icp_wdf.getFitnessScore (1000) << std::endl; icp_wdf.getFinalTransformation (); /// Final ICP transformation is obtained by multiplying initial transformed with icp refinement */ /*------BEST------------- icp.getMaximumIterations 50 icp.getRANSACOutlierRejectionThreshold() 0.02 icp.getMaxCorrespondenceDistance() 0.03 icp.getTransformationEpsilon () 1e-09 icp.getEuclideanFitnessEpsilon () -1.79769e+308 score: 0.000164332 --------------------- //Normal (non modified) icp for reference pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icp; icp.setMaximumIterations (20); std::cerr << "icp.getMaximumIterations " << icp.getMaximumIterations() << std::endl; icp.setRANSACOutlierRejectionThreshold(0.05); std::cerr << "icp.getRANSACOutlierRejectionThreshold() " << icp.getRANSACOutlierRejectionThreshold() << std::endl; icp.setMaxCorrespondenceDistance(0.05); std::cerr << "icp.getMaxCorrespondenceDistance() " << icp.getMaxCorrespondenceDistance() << std::endl; //only used for convergence test icp.setTransformationEpsilon (0); std::cerr << "icp.getTransformationEpsilon () " << icp.getTransformationEpsilon () << std::endl; //only used for convergence test std::cerr << "icp.getEuclideanFitnessEpsilon () " << icp.getEuclideanFitnessEpsilon () << std::endl; icp.setInputCloud(featureCloudSource); icp.setInputTarget(featureCloudTarget); pcl::PointCloud<pcl::PointXYZRGBNormal> Final_reference; std::cout << "ICP has starts with a score of" << icp.getFitnessScore() << std::endl; ROS_INFO("Performing standard icp....."); icp.align(Final_reference);//, guess); std::cout << "ICP has finished with converge flag of:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; */ /* ROS_INFO("Writing output clouds..."); transformPointCloud (*featureCloudSourceTemp, *cloud_converg_sparse_all, icp_wdf.getFinalTransformation()); transformPointCloud (*featureCloudSourceTemp, *cloud_converg_sparse_correspond, icp_wdf.getFinalTransformation()); transformPointCloud (*cloud_source, *cloud_converg_dense, icp_wdf.getFinalTransformation()); transformPointCloud (*cloud_source, *cloud_ransac_estimation, ransacInverse); // remove non correspondences from feature clouds as an addition output uint8_t col=3; // *cloud_converg_sparse_correspond = *cloud_converg_sparse_all; cloud_converg_sparse_correspond->points.resize(indicesSourceSmall.size()); cloud_converg_sparse_correspond->height = 1; cloud_converg_sparse_correspond->width = (int)indicesSourceSmall.size(); cloud_target_sparse_correspond->points.resize(indicesTargetSmall.size()); cloud_target_sparse_correspond->height = 1; cloud_target_sparse_correspond->width = (int)indicesTargetSmall.size(); for (size_t cloudId = 0; cloudId < indicesSourceSmall.size(); ++cloudId) { cloud_converg_sparse_correspond->points[cloudId].x = cloud_converg_sparse_all->points[indicesSourceSmall[cloudId]].x; cloud_converg_sparse_correspond->points[cloudId].y = cloud_converg_sparse_all->points[indicesSourceSmall[cloudId]].y; cloud_converg_sparse_correspond->points[cloudId].z = cloud_converg_sparse_all->points[indicesSourceSmall[cloudId]].z; cloud_converg_sparse_correspond->points[cloudId].r = col; cloud_converg_sparse_correspond->points[cloudId].g = col; cloud_converg_sparse_correspond->points[cloudId].b = col; cloud_target_sparse_correspond->points[cloudId].x = featureCloudTarget->points[indicesTargetSmall[cloudId]].x; cloud_target_sparse_correspond->points[cloudId].y = featureCloudTarget->points[indicesTargetSmall[cloudId]].y; cloud_target_sparse_correspond->points[cloudId].z = featureCloudTarget->points[indicesTargetSmall[cloudId]].z; cloud_target_sparse_correspond->points[cloudId].r = col; cloud_target_sparse_correspond->points[cloudId].g = col; cloud_target_sparse_correspond->points[cloudId].b = col; //std::cerr << "point " << cloudId << "\n "; } pcl::PCDWriter writer; writer.write ("cloud1-out.pcd", *cloud_source, false); writer.write ("cloud2-out.pcd", *cloud_target, false); writer.write ("normals-source.pcd", *cloud_source_normals, false); writer.write ("normals-target.pcd", *cloud_source_normals, false); writer.write ("feature-source.pcd", *featureCloudSource, false); writer.write ("feature-target.pcd", *featureCloudTarget, false); writer.write ("converged-cloud.pcd", *cloud_converg_dense, false); writer.write ("converged-feature-all.pcd", *cloud_converg_sparse_all, false); writer.write ("converged-feature-correspond.pcd", *cloud_converg_sparse_correspond, false); writer.write ("target-feature-correspond.pcd", *cloud_target_sparse_correspond, false); writer.write ("ransac_estimation.pcd", *cloud_ransac_estimation, false); // writer.write ("converged-reference.pcd", Final_reference, false); */ return (0); }
void run(string path, const int &selectSample) { std::cout << "SphereGraphSLAM::run... " << std::endl; // // Create the topological arrangement object // TopologicalMap360 topologicalMap(Map); // Map.currentArea = 0; // // Get the calibration matrices for the different sensors // Calib360 calib; // calib.loadExtrinsicCalibration(); // calib.loadIntrinsicCalibration(); // // Create registration object // RegisterRGBD360 registerer(mrpt::format("%s/config_files/configLocaliser_sphericalOdometry.ini", PROJECT_SOURCE_PATH)); // Filter the point clouds before visualization FilterPointCloud filter; int frame = 282; // Skip always the first frame, which sometimes contains errors int frameOrder = 0; Eigen::Matrix4f currentPose = Eigen::Matrix4f::Identity(); string fileName = path + mrpt::format("/sphere_images_%d.bin",frame); // Load first frame Frame360* frame360 = new Frame360(&calib); frame360->loadFrame(fileName); frame360->undistort(); frame360->stitchSphericalImage(); frame360->buildSphereCloud(); frame360->getPlanes(); //frame360->id = frame; frame360->id = frameOrder; frame360->node = Map.currentArea; nearestKF = frameOrder; filter.filterEuclidean(frame360->sphereCloud); Map.addKeyframe(frame360,currentPose); // Map.vOptimizedPoses.push_back( currentPose ); Map.vSelectedKFs.push_back(0); Map.vTrajectoryIncrements.push_back(0); // Topological Partitioning Map.vsAreas.push_back( std::set<unsigned>() ); Map.vsAreas[Map.currentArea].insert(frameOrder); Map.vsNeighborAreas.push_back( std::set<unsigned>() ); // Vector with numbers of neighbor areas (topological nodes) Map.vsNeighborAreas[Map.currentArea].insert(Map.currentArea); topologicalMap.vSSO.push_back( mrpt::math::CMatrix(1,1) ); topologicalMap.vSSO[Map.currentArea](frameOrder,frameOrder) = 0.0; frame += selectSample; fileName = path + mrpt::format("/sphere_images_%d.bin",frame); // Start visualizer Map360_Visualizer Viewer(Map,1); // Graph-SLAM // std::cout << "\t mmConnectionKFs " << Map.mmConnectionKFs.size() << " \n"; // LoopClosure360 loopCloser(Map); // float areaMatched, currentPlanarArea; // Frame360 *candidateKF = new Frame360(&calib); // A reference to the last registered frame to add as a keyframe when needed // std::pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> > candidateKF_connection; // The register information of the above GraphOptimizer optimizer; optimizer.setRigidTransformationType(GraphOptimizer::SixDegreesOfFreedom); std::cout << "Added vertex: "<< optimizer.addVertex(currentPose.cast<double>()) << std::endl; bool bHasNewLC = false; Frame360 *candidateKF = NULL; // A reference to the last registered frame to add as a keyframe when needed std::pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> > candidateKF_connection; // The register information of nearestKF float candidateKF_sso; // while( true ) // boost::this_thread::sleep (boost::posix_time::milliseconds (10)); bool bPrevKFSelected = true, bGoodTracking = true; int lastTrackedKF = 0; // Count the number of not tracked frames after the last registered one // Dense RGB-D alignment RegisterPhotoICP align360; align360.setNumPyr(5); align360.useSaliency(false); // align360.setVisualization(true); align360.setGrayVariance(3.f/255); double selectKF_ICPdist = 0.9; // double selectKF_ICPdist = 1.1; double thresholdConnections = 2.5; // Distance in meters to evaluate possible connections Eigen::Matrix4f rigidTransf_dense = Eigen::Matrix4f::Identity(); Eigen::Matrix4f rigidTransf_dense_ref = rigidTransf_dense; // The reference of the spherical image and the point Clouds are not the same! I should always use the same coordinate system (TODO) float angleOffset = 157.5; Eigen::Matrix4f rotOffset = Eigen::Matrix4f::Identity(); rotOffset(1,1) = rotOffset(2,2) = cos(angleOffset*PI/180); rotOffset(1,2) = sin(angleOffset*PI/180); rotOffset(2,1) = -rotOffset(1,2); while( fexists(fileName.c_str()) ) { // Load pointCloud // if(!bPrevKFSelected) { cout << "Frame " << fileName << endl; frame360 = new Frame360(&calib); frame360->loadFrame(fileName); frame360->undistort(); frame360->stitchSphericalImage(); frame360->buildSphereCloud(); frame360->getPlanes(); frame360->id = frameOrder+1; } // cout << " Load Keyframe \n" << endl; // The next frame to process frame += selectSample; fileName = path + mrpt::format("/sphere_images_%d.bin",frame); // Register the pair of frames bGoodTracking = registerer.RegisterPbMap(Map.vpSpheres[nearestKF], frame360, MAX_MATCH_PLANES, RegisterRGBD360::PLANAR_3DoF); Matrix4f trackedPosePbMap = registerer.getPose(); cout << "bGoodTracking " << bGoodTracking << " with " << nearestKF << ". Distance " << registerer.getPose().block(0,3,3,1).norm() << " entropy " << registerer.calcEntropy() << " matches " << registerer.getMatchedPlanes().size() << " area " << registerer.getAreaMatched() << endl; // // If the registration is good, do not evaluate this KF and go to the next // if( bGoodTracking && registerer.getMatchedPlanes().size() >= min_planes_registration && registerer.getAreaMatched() > 12 )//&& registerer.getPose().block(0,3,3,1).norm() < 1.5 ) // // if( bGoodTracking && registerer.getMatchedPlanes().size() > 6 && registerer.getPose().block(0,3,3,1).norm() < 1.0 ) // { // float dist_odometry = 0; // diff_roatation // if(candidateKF) // dist_odometry = (candidateKF_connection.first.block(0,3,3,1) - trackedPosePbMap.block(0,3,3,1)).norm(); // // else // // dist_odometry = trackedPosePbMap.norm(); // if( dist_odometry < max_translation_odometry ) // Check that the registration is coherent with the camera movement (i.e. in odometry the registered frames must lay nearby) // { // if(trackedPosePbMap.block(0,3,3,1).norm() > min_dist_keyframes) // Minimum distance to select a possible KF // { // cout << "Set candidateKF \n"; // if(candidateKF) // delete candidateKF; // Delete previous candidateKF // candidateKF = frame360; // candidateKF_connection.first = trackedPosePbMap; // candidateKF_connection.second = registerer.getInfoMat(); // candidateKF_sso = registerer.getAreaMatched() / registerer.areaSource; // } // { boost::mutex::scoped_lock updateLockVisualizer(Viewer.visualizationMutex); // Viewer.currentLocation.matrix() = (currentPose * trackedPosePbMap); // Viewer.bDrawCurrentLocation = true; // } // bPrevKFSelected = false; // lastTrackedKF = 0; // } // continue; // Eval next frame // } if(bGoodTracking && registerer.getMatchedPlanes().size() >= 6 && registerer.getAreaMatched() > 12) { delete frame360; rigidTransf_dense_ref = rotOffset * trackedPosePbMap * rotOffset.inverse(); { boost::mutex::scoped_lock updateLockVisualizer(Viewer.visualizationMutex); Viewer.currentLocation.matrix() = (currentPose * rigidTransf_dense); Viewer.bDrawCurrentLocation = true; } cout << " skip frame PbMap " << endl; continue; } cout << "Align Spheres " << frame360->id << " and " << Map.vpSpheres[nearestKF]->id << endl; double time_start_dense = pcl::getTime(); align360.setTargetFrame(Map.vpSpheres[nearestKF]->sphereRGB, Map.vpSpheres[nearestKF]->sphereDepth); align360.setSourceFrame(frame360->sphereRGB, frame360->sphereDepth); // rigidTransf_dense_ref = rotOffset * registerer.getPose() * rotOffset.inverse(); align360.alignFrames360(rigidTransf_dense_ref, RegisterPhotoICP::PHOTO_DEPTH); // PHOTO_CONSISTENCY / DEPTH_CONSISTENCY / PHOTO_DEPTH Matrix4f relPoseDense = registerer.getPose(); Eigen::Matrix4f rigidTransf_dense_ref_prev = rigidTransf_dense_ref; rigidTransf_dense_ref = align360.getOptimalPose(); rigidTransf_dense = rotOffset.inverse() * rigidTransf_dense_ref * rotOffset; double time_end_dense = pcl::getTime(); //std::cout << "Dense " << (time_end_dense - time_start_dense) << std::endl; double depth_residual = align360.avDepthResidual; cout << " Residuals: " << align360.avPhotoResidual << " " << align360.avDepthResidual; //cout << " regist \n" << rigidTransf_dense << endl; { boost::mutex::scoped_lock updateLockVisualizer(Viewer.visualizationMutex); Viewer.currentLocation.matrix() = (currentPose * rigidTransf_dense); Viewer.bDrawCurrentLocation = true; } if(align360.avDepthResidual < selectKF_ICPdist && isOdometryContinuousMotion(rigidTransf_dense_ref_prev, rigidTransf_dense_ref, 0.2)) { // assert(align360.avDepthResidual < 1.5); delete frame360; // bPrevKFSelected = false; cout << " skip frame " << endl; continue; } // The last frame is a candidate for a KF candidateKF = frame360; candidateKF_connection.first = rigidTransf_dense; candidateKF_connection.second = align360.getHessian(); candidateKF_sso = align360.SSO; // Check registration with nearby keyframes vector<connection> vConnections; unsigned kf; for( kf=0; kf < Map.vpSpheres.size(); kf++) // if(Map.vpSpheres[kf]->node == Map.currentArea && kf != nearestKF) // || Map.vsAreas[Map.currentArea].count(kf)) if((Map.vpSpheres[kf]->node == Map.currentArea || isInNeighbourSubmap(Map.currentArea,kf)) && kf != nearestKF) { Eigen::Matrix4f relativePose = Map.vpSpheres[kf]->pose.inverse() * currentPose * rigidTransf_dense; if(relativePose.block(0,3,3,1).norm() < thresholdConnections) // If the KFs are closeby { // std::cout << "Check loop closure between " << kf << " the current frame " << "\n"; // bool bGoodRegistration = registerer.RegisterPbMap(Map.vpSpheres[kf], frame360, 25, RegisterRGBD360::RegisterRGBD360::PLANAR_3DoF); // if(bGoodRegistration && registerer.getMatchedPlanes().size() > 5 && registerer.getAreaMatched() > 15.0) { // std::cout << "Loop closed between " << newFrameID << " and " << compareLocalIdx << " matchedArea " << registerer.getAreaMatched() << std::endl; // Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> informationMatrix = registerer.getAreaMatched() * registerer.getInfoMat().cast<double>(); // relativePose = registerer.getPose(); // Eigen::Matrix4f initRigidTransfDense = rotOffset * relativePose * rotOffset.inverse(); align360.setTargetFrame(Map.vpSpheres[kf]->sphereRGB, Map.vpSpheres[kf]->sphereDepth); align360.setSourceFrame(frame360->sphereRGB, frame360->sphereDepth); Eigen::Matrix4f initRigidTransfDense = rotOffset * relativePose * rotOffset.inverse(); align360.alignFrames360(initRigidTransfDense, RegisterPhotoICP::PHOTO_DEPTH); // PHOTO_CONSISTENCY / DEPTH_CONSISTENCY / PHOTO_DEPTH Matrix4f relPoseDense = registerer.getPose(); rigidTransf_dense_ref_prev = rigidTransf_dense_ref; rigidTransf_dense_ref = align360.getOptimalPose(); if(isOdometryContinuousMotion(rigidTransf_dense_ref_prev, rigidTransf_dense_ref, 0.2)) continue; Eigen::Matrix4f rigidTransf_denseKF = rotOffset.inverse() * rigidTransf_dense_ref * rotOffset; cout << "CHECK WITH " << kf << " nearestKF " << nearestKF << " avDepthResidual " << align360.avDepthResidual << " sso " << align360.SSO << endl; // cout << "dist " << (Map.vpSpheres[kf]->pose.block(0,3,3,1)-frame360->pose.block(0,3,3,1)).norm() << " " << (initRigidTransfDense.block(0,3,3,1)).norm() << endl; if(align360.avDepthResidual < selectKF_ICPdist) break; else if(align360.avDepthResidual < 1.8) // Keep connection { connection Connection; Connection.KF_id = kf; Connection.geomConnection = pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> >(rigidTransf_denseKF, align360.getHessian()); Connection.sso = align360.SSO; vConnections.push_back(Connection); } //else { //std::cout << "Check loop closure between " << kf << " the current frame " << "\n"; bool bGoodRegistration = registerer.RegisterPbMap(Map.vpSpheres[kf], frame360, 25, RegisterRGBD360::RegisterRGBD360::PLANAR_3DoF); if(bGoodRegistration && registerer.getMatchedPlanes().size() > 5 && registerer.getAreaMatched() > 25.0) { connection Connection; Connection.KF_id = kf; Connection.geomConnection = pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> >(registerer.getPose(), registerer.getAreaMatched() * registerer.getInfoMat()); Connection.sso = align360.SSO; vConnections.push_back(Connection); } } } } } if(align360.avDepthResidual < selectKF_ICPdist) { cout << "\t align360.avDepthResidual " << align360.avDepthResidual << " " << selectKF_ICPdist << " " << kf << endl; if(align360.avDepthResidual < depth_residual) { currentPose = Map.vpSpheres[kf]->pose; nearestKF = kf; Viewer.currentSphere = nearestKF; cout << "\t nearestKF " << nearestKF << endl; } delete frame360; cout << "skip frame 2 " << endl; continue; } cout << "SSO " << align360.SSO << endl; cout << "Select keyframe " << frame360->id << endl; cout << "Information \n" << candidateKF_connection.second << endl; rigidTransf_dense_ref = Eigen::Matrix4f::Identity(); ++lastTrackedKF; // if(bPrevKFSelected) // If the previous frame was a keyframe, just try to register the next frame as usually // { // std::cout << "Tracking lost " << lastTrackedKF << "\n"; // // bGoodTracking = true; // Read a new frame in the next iteration // bPrevKFSelected = false; // continue; // } // if(lastTrackedKF > 3) // { // std::cout << "\n Launch Relocalization \n"; // double time_start = pcl::getTime(); // if(relocalizer.relocalize(frame360) != -1) // { // double time_end = pcl::getTime(); // std::cout << "Relocalization in " << Map.vpSpheres.size() << " KFs map took " << double (time_end - time_start) << std::endl; // lastTrackedKF = 0; // candidateKF = frame360; // candidateKF_connection.first = relocalizer.registerer.getPose(); // candidateKF_connection.second = relocalizer.registerer.getInfoMat(); // nearestKF = relocalizer.relocKF; // currentPose = Map.vTrajectoryPoses[nearestKF]; //Map.vOptimizedPoses // Viewer.currentSphere = nearestKF; // std::cout << "Relocalized with " << nearestKF << " \n\n\n"; // continue; // } // } // if(!candidateKF) // { // std::cout << "_Tracking lost " << lastTrackedKF << "\n"; // continue; // } // if( !shouldSelectKeyframe(candidateKF, candidateKF_connection, frame360) ) // { // cout << " Do not add a keyframe yet. Now the camera is around KF " << nearestKF << endl; // candidateKF = NULL; // currentPose = Map.vTrajectoryPoses[nearestKF]; //Map.vOptimizedPoses // Viewer.currentSphere = nearestKF; // frame -= selectSample; // fileName = path + mrpt::format("/sphere_images_%d.bin",frame); // // lastTrackedKF = 0; // // bGoodTracking = true; // Read a new frame in the next iteration // // bPrevKFSelected = true; // This avoids selecting a KF for which there is no registration // continue; // } // // bPrevKFSelected = true; // frame -= selectSample; // fileName = path + mrpt::format("/sphere_images_%d.bin",frame); currentPose = currentPose * candidateKF_connection.first; std::cout<< "Added vertex: "<< optimizer.addVertex(currentPose.cast<double>()) << std::endl; std::cout<< "Added addEdge:" << nearestKF << " " << Map.vpSpheres.size() << "\n" << candidateKF_connection.first.cast<double>() << "\n"; optimizer.addEdge(nearestKF, Map.vpSpheres.size(), candidateKF_connection.first.cast<double>(), candidateKF_connection.second.cast<double>()); std::cout<< " Edge PbMap: " << bGoodTracking << " " << registerer.getMatchedPlanes().size() << " " << registerer.getAreaMatched() << " " << diffRotation(trackedPosePbMap, rigidTransf_dense) << " " << difTranslation(trackedPosePbMap, rigidTransf_dense) << "\n"; if(bGoodTracking && registerer.getMatchedPlanes().size() >= 4 && registerer.getAreaMatched() > 6) if(diffRotation(trackedPosePbMap, rigidTransf_dense) < 5 && difTranslation(trackedPosePbMap, rigidTransf_dense) < 0.1) // If the difference is less than 5ยบ and 10 cm, add the PbMap connection { optimizer.addEdge(nearestKF, Map.vpSpheres.size(), trackedPosePbMap.cast<double>(), registerer.getInfoMat().cast<double>()); std::cout<< "Added addEdge PbMap\n"; std::cout<< "Edge PbMap\n" << registerer.getInfoMat() << endl; std::cout<< "Edge Dense" << candidateKF_connection.second << endl; } Map.vTrajectoryIncrements.push_back(Map.vTrajectoryIncrements.back() + candidateKF_connection.first.block(0,3,3,1).norm()); // Filter cloud filter.filterEuclidean(candidateKF->sphereCloud); std::cout<< "filterEuclidean:\n"; cout << "\tGet previous frame as a keyframe " << frameOrder+1 << " dist " << candidateKF_connection.first.norm() << " candidateKF_sso " << candidateKF_sso << endl; // Add new keyframe candidateKF->id = ++frameOrder; candidateKF->node = Map.currentArea; // Update topologicalMap int newLocalFrameID = Map.vsAreas[Map.currentArea].size(); topologicalMap.addKeyframe(Map.currentArea); std::cout<< "newSizeLocalSSO "<< newLocalFrameID+1 << std::endl; // int newSizeLocalSSO = newLocalFrameID+1; // topologicalMap.vSSO[Map.currentArea].setSize(newSizeLocalSSO,newSizeLocalSSO); // topologicalMap.vSSO[Map.currentArea](newLocalFrameID,newLocalFrameID) = 0.0; // // Re-adjust size of adjacency matrices // for(set<unsigned>::iterator it=Map.vsNeighborAreas[Map.currentArea].begin(); it != Map.vsNeighborAreas[Map.currentArea].end(); it++) // { // assert(*it != Map.currentArea); // if(*it < Map.currentArea) // { // cout << " sizeSSO neig " << topologicalMap.mmNeigSSO[*it][Map.currentArea].getRowCount() << endl; // topologicalMap.mmNeigSSO[*it][Map.currentArea].setSize(5, newSizeLocalSSO); // } // else // { // topologicalMap.mmNeigSSO[Map.currentArea][*it].setSize(newSizeLocalSSO, topologicalMap.mmNeigSSO[*it][Map.currentArea].getRowCount()); // } // } // Track the current position. TODO: (detect inconsistencies) int compareLocalIdx = newLocalFrameID-1; std::cout<< "compareLocalIdx "<< compareLocalIdx << std::endl; // Lock map to add a new keyframe {boost::mutex::scoped_lock updateLock(Map.mapMutex); Map.addKeyframe(candidateKF, currentPose); //Map.vOptimizedPoses.push_back( Map.vOptimizedPoses[nearestKF] * candidateKF_connection.first ); std::cout << "\t mmConnectionKFs loop " << frameOrder << " " << nearestKF << " \n"; Map.mmConnectionKFs[frameOrder] = std::map<unsigned, std::pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> > >(); Map.mmConnectionKFs[frameOrder][nearestKF] = std::pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> >(candidateKF_connection.first, candidateKF_connection.second); Map.vsAreas[Map.currentArea].insert(frameOrder); } // Update candidateKF cout << "Update candidateKF " << endl; candidateKF = NULL; nearestKF = frameOrder; Viewer.currentSphere = nearestKF; // Viewer.bFreezeFrame = false; cout << "Add tracking SSO " << endl; if((topologicalMap.vSSO[Map.currentArea].getRowCount() <= newLocalFrameID) && (topologicalMap.vSSO[Map.currentArea].getRowCount() <= compareLocalIdx)) cout << "\n\n\t ERROR vSSO\n"; topologicalMap.vSSO[Map.currentArea](newLocalFrameID,compareLocalIdx) = topologicalMap.vSSO[Map.currentArea](compareLocalIdx,newLocalFrameID) = candidateKF_sso; cout << "SSO is " << topologicalMap.vSSO[Map.currentArea](newLocalFrameID,compareLocalIdx) << endl; // Add for Map.mmConnectionKFs in the previous frames. if(vConnections.size() > 0)// && frameOrder !=3 && frameOrder !=4) { bHasNewLC = true; cout << "Add vConnections " << vConnections.size() << endl; for(unsigned link=0; link < vConnections.size(); link++) { // cout << "Add vConnections between " << frameOrder << " " << vConnections[link].KF_id << "\n"; // cout << vConnections[link].KF_id << " sso " << vConnections[link].sso << "\n"; Map.mmConnectionKFs[frameOrder][vConnections[link].KF_id] = vConnections[link].geomConnection; // cout << "Add vConnections_ A\n"; topologicalMap.addConnection(unsigned(frameOrder), vConnections[link].KF_id, vConnections[link].sso); // cout << "Add vConnections_ B\n"; optimizer.addEdge(vConnections[link].KF_id, unsigned(frameOrder), vConnections[link].geomConnection.first.cast<double>(), vConnections[link].geomConnection.second.cast<double>()); // cout << "frameOrder " << frameOrder << " " << Map.vpSpheres.size() << endl; } } // // Calculate distance of furthest registration (approximated, we do not know if the last frame compared is actually the furthest) // std::map<unsigned, pair<unsigned,Eigen::Matrix4f> >::iterator itFurthest = Map.mmConnectionKFs[frameOrder].begin(); // Aproximated // cout << "Furthest distance is " << itFurthest->second.second.block(0,3,3,1).norm() << " diffFrameNum " << newLocalFrameID - itFurthest->first << endl; // // Synchronize topologicalMap with loopCloser // while( !loopCloser.connectionsLC.empty() ) // { // std::map<unsigned, std::map<unsigned, float> >::iterator it_connectLC1 = loopCloser.connectionsLC.begin(); // while( !it_connectLC1->second.empty() ) // { // std::map<unsigned, float>::iterator it_connectLC2 = it_connectLC1->second.begin(); // if(Map.vpSpheres[it_connectLC1->first]->node != Map.currentArea || Map.vpSpheres[it_connectLC2->first]->node != Map.currentArea) // { // loopCloser.connectionsLC.erase(it_connectLC1); // continue; // } // int localID1 = std::distance(Map.vsAreas[Map.currentArea].begin(), Map.vsAreas[Map.currentArea].find(it_connectLC1->first)); // int localID2 = std::distance(Map.vsAreas[Map.currentArea].begin(), Map.vsAreas[Map.currentArea].find(it_connectLC2->first)); // cout << "Add LC SSO " << topologicalMap.vSSO[Map.currentArea].getRowCount() << " " << localID1 << " " << localID2 << endl; // topologicalMap.vSSO[Map.currentArea](localID1,localID2) = topologicalMap.vSSO[Map.currentArea](localID2,localID1) = // it_connectLC2->second; // it_connectLC1->second.erase(it_connectLC2); // } // loopCloser.connectionsLC.erase(it_connectLC1); // } // Optimize graphSLAM with g2o if(bHasNewLC) { std::cout << "Optimize graph SLAM \n"; double time_start = pcl::getTime(); //Optimize the graph // for(int j=0; j < Map.vpSpheres.size(); j++) // { //// std::cout << "optimizedPose " << poseIdx << " submap " << Map.vpSpheres[j]->node << " same? " << Map.vOptimizedPoses[j]==Map.vTrajectoryPoses[j] ? 1 : 0 << std::endl; // g2o::OptimizableGraph::Vertex *vertex_j = optimizer.optimizer.vertex(j); // if(Map.vpSpheres[j]->node == Map.currentArea && j != Map.vSelectedKFs[Map.currentArea]) // vertex_j->setFixed(false); // else // vertex_j->setFixed(true); // } //VertexContainer vertices = optimizer.activeVertices.activeVertices(); optimizer.optimizeGraph(); double time_end = pcl::getTime(); std::cout << " Optimize graph SLAM " << Map.vpSpheres.size() << " took " << double (time_end - time_start) << " s.\n"; // for(int j=0; j < Map.vpSpheres.size(); j++) // std::cout << "optimizedPose " << j << endl << Map.vOptimizedPoses[j] << endl; //Update the optimized poses (for loopClosure detection & visualization) { boost::mutex::scoped_lock updateLock(Map.mapMutex); optimizer.getPoses(Map.vOptimizedPoses); //Viewer.currentLocation.matrix() = Map.vOptimizedPoses.back(); } // for(int j=0; j < Map.vpSpheres.size(); j++) // std::cout << "optimizedPose " << j << endl << Map.vOptimizedPoses[j] << endl; //Update the optimized pose //Eigen::Matrix4f &newPose = Map.vOptimizedPoses.back(); // std::cout << "First pose optimized \n" << Map.vOptimizedPoses[0] << std::endl; // assert(Map.vpSpheres.size() == Map.vOptimizedPoses.size()); for(int j=0; j < Map.vpSpheres.size(); j++) std::cout << "optimizedPose " << j << " " << optimizer.optimizer.vertex(j)->fixed() << " submap " << Map.vpSpheres[j]->node << std::endl; //<< " same? " << Map.vOptimizedPoses[j]==Map.vTrajectoryPoses[j] ? 1 : 0 << std::endl; // std::cout << "newPose \n" << Map.vOptimizedPoses.back() << "\n previous \n" << currentPose << std::endl; currentPose = Map.vOptimizedPoses.back(); bHasNewLC = false; } // Visibility divission (Normalized-cut) if(newLocalFrameID % 4 == 0) { // for(unsigned i=0; i < Map.vTrajectoryPoses.size(); i++) // cout << "pose " << i << "\n" << Map.vTrajectoryPoses[i] << endl; double time_start = pcl::getTime(); // cout << "Eval partitions\n"; topologicalMap.Partitioner(); // // Set only the current submap's keyframes as optimizable vertex in Graph-SLAM // for(int j=0; j < Map.vpSpheres.size(); j++) // { //// std::cout << "optimizedPose " << poseIdx << " submap " << Map.vpSpheres[j]->node << " same? " << Map.vOptimizedPoses[j]==Map.vTrajectoryPoses[j] ? 1 : 0 << std::endl; // g2o::OptimizableGraph::Vertex *vertex_j = optimizer.optimizer.vertex(j); // if(Map.vpSpheres[j]->node == Map.currentArea && j != Map.vSelectedKFs[Map.currentArea]) // vertex_j->setFixed(false); // else // vertex_j->setFixed(true); // } // for(unsigned i=0; i < Map.vTrajectoryPoses.size(); i++) // cout << "pose " << i << "\n" << Map.vTrajectoryPoses[i] << endl; cout << "\tPlaces\n"; for( unsigned node = 0; node < Map.vsNeighborAreas.size(); node++ ) { cout << "\t" << node << ":"; for( set<unsigned>::iterator it = Map.vsNeighborAreas[node].begin(); it != Map.vsNeighborAreas[node].end(); it++ ) cout << " " << *it; cout << endl; } double time_end = pcl::getTime(); std::cout << " Eval partitions took " << double (time_end - time_start)*10e3 << "ms" << std::endl; } //if(Map.vsNeighborAreas.size() > 1) //mrpt::system::pause(); // Viewer.bFreezeFrame = false; } // while (!Viewer.viewer.wasStopped() ) // boost::this_thread::sleep (boost::posix_time::milliseconds (10)); cout << "Path length " << Map.vTrajectoryIncrements.back() << endl; }
// This function decides to select a new keyframe when the proposed keyframe does not have keyframes too near/related bool shouldSelectKeyframe(Frame360 *candidateKF, std::pair<Eigen::Matrix4f, Eigen::Matrix<float,6,6> > candidateKF_connection, Frame360* currentFrame) { cout << "shouldSelectKeyframe ...\n"; double time_start = pcl::getTime(); RegisterPhotoICP align360; // Dense RGB-D alignment align360.setNumPyr(5); align360.useSaliency(false); align360.setVisualization(true); align360.setGrayVariance(3.f/255); // The reference of the spherical image and the point Clouds are not the same! I should always use the same coordinate system (TODO) float angleOffset = 157.5; Eigen::Matrix4f rotOffset = Eigen::Matrix4f::Identity(); rotOffset(1,1) = rotOffset(2,2) = cos(angleOffset*PI/180); rotOffset(1,2) = sin(angleOffset*PI/180); rotOffset(2,1) = -rotOffset(1,2); cout << "Align Sphere " << endl; double time_start_dense = pcl::getTime(); align360.setTargetFrame(Map.vpSpheres[nearestKF]->sphereRGB, Map.vpSpheres[nearestKF]->sphereDepth); align360.setSourceFrame(candidateKF->sphereRGB, candidateKF->sphereDepth); // align360.alignFrames360(Eigen::Matrix4f::Identity(), RegisterPhotoICP::PHOTO_CONSISTENCY); // PHOTO_CONSISTENCY / DEPTH_CONSISTENCY / PHOTO_DEPTH Matrix4f relPoseDense = registerer.getPose(); align360.alignFrames360(rotOffset * candidateKF_connection.first * rotOffset.inverse(), RegisterPhotoICP::PHOTO_DEPTH); // PHOTO_CONSISTENCY / DEPTH_CONSISTENCY / PHOTO_DEPTH Matrix4f relPoseDense = registerer.getPose(); Eigen::Matrix4f rigidTransf_dense = rotOffset.inverse() * align360.getOptimalPose() * rotOffset; double time_end_dense = pcl::getTime(); std::cout << "Dense " << (time_end_dense - time_start_dense) << std::endl; cout << "Dense regist \n" << rigidTransf_dense << " \nRegist-PbMap \n" << candidateKF_connection.first << endl; // math::mrpt::CPose3D if(!rigidTransf_dense.isApprox(candidateKF_connection.first,1e-1)) { std::cout << "shouldSelectKeyframe INVALID KF\n"; return false; } else { candidateKF_connection.first = rigidTransf_dense; } double dist_candidateKF = candidateKF_connection.first.block(0,3,3,1).norm(); Eigen::Matrix4f candidateKF_pose = Map.vTrajectoryPoses[nearestKF] * candidateKF_connection.first; map<float,unsigned> nearest_KFs; for(std::set<unsigned>::iterator itKF = Map.vsAreas[Map.currentArea].begin(); itKF != Map.vsAreas[Map.currentArea].end(); itKF++) // Set the iterator to the previous KFs not matched yet { if(*itKF == nearestKF) continue; double dist_to_prev_KF = (candidateKF_pose.block(0,3,3,1) - Map.vTrajectoryPoses[*itKF].block(0,3,3,1)).norm(); cout << *itKF << " dist_to_prev_KF " << dist_to_prev_KF << " dist_candidateKF " << dist_candidateKF << endl; if( dist_to_prev_KF < dist_candidateKF ) // Check if there are even nearer keyframes nearest_KFs[dist_to_prev_KF] = *itKF; } for(map<float,unsigned>::iterator itNearKF = nearest_KFs.begin(); itNearKF != nearest_KFs.end(); itNearKF++) // Set the iterator to the previous KFs not matched yet { cout << "itNearKF " << itNearKF->first << " " << itNearKF->second << endl; bool bGoodRegistration = registerer.RegisterPbMap(Map.vpSpheres[itNearKF->second], candidateKF, MAX_MATCH_PLANES, RegisterRGBD360::PLANAR_3DoF); if(bGoodRegistration && registerer.getMatchedPlanes().size() >= min_planes_registration && registerer.getAreaMatched() > 6 ) { nearestKF = itNearKF->second; double time_end = pcl::getTime(); std::cout << "shouldSelectKeyframe INVALID KF. took " << double (time_end - time_start) << std::endl; return false; } } double time_end = pcl::getTime(); std::cout << "shouldSelectKeyframe VALID KF. took " << double (time_end - time_start) << std::endl; // cout << "shouldSelectKeyframe VALID KF\n"; return true; }
void getTestDataFromBag(PointCloudPtr cloud_source, PointCloudPtr cloud_target, PointCloudPtr featureCloudSource, std::vector<int> &indicesSource, PointCloudPtr featureCloudTarget, std::vector<int> &indicesTarget, Eigen::Matrix4f &initialTransform, int rosMessageNumber) { sensor_msgs::PointCloud2::Ptr cloud_source_filtered (new sensor_msgs::PointCloud2 ()); sensor_msgs::PointCloud2::Ptr cloud_target_filtered (new sensor_msgs::PointCloud2 ()); PointCloudPtr cloud_ransac_estimation (new PointCloud); pcl::PCDWriter writer; std::stringstream filename; rosbag::Bag bag; bag.open("/media/burg/data/bagfiles/bench1-ManySweeps4-lowfeatureThresNode2.bag", rosbag::bagmode::Read); std::vector<std::string> topics; topics.push_back(std::string("/feature_match_out_topic")); rosbag::View view(bag, rosbag::TopicQuery(topics)); int i = 1; BOOST_FOREACH(rosbag::MessageInstance const m, view) { if(( i == rosMessageNumber) || (rosMessageNumber==0)) { pcl_tutorial::featureMatch::ConstPtr fm = m.instantiate<pcl_tutorial::featureMatch>(); ROS_INFO("Converting point cloud message to local pcl clouds"); sensor_msgs::PointCloud2::Ptr cloud_source_temp_Ptr (new sensor_msgs::PointCloud2 (fm->sourcePointcloud)); sensor_msgs::PointCloud2::Ptr cloud_target_temp_Ptr (new sensor_msgs::PointCloud2 (fm->targetPointcloud)); voxFilterPointCloud(cloud_source_temp_Ptr, cloud_source_filtered); voxFilterPointCloud(cloud_target_temp_Ptr, cloud_target_filtered); ROS_INFO("Converting dense clouds"); pcl::fromROSMsg(*cloud_source_filtered, *cloud_source); pcl::fromROSMsg(*cloud_target_filtered, *cloud_target); ROS_INFO("Converting sparse clouds"); pcl::fromROSMsg(fm->sourceFeatureLocations, *featureCloudSource); pcl::fromROSMsg(fm->targetFeatureLocations, *featureCloudTarget); ROS_INFO("Converting geometry message to eigen4f"); tf::Transform trans; tf::transformMsgToTF(fm->featureTransform,trans); initialTransform = EigenfromTf(trans); ROS_INFO_STREAM("transform from ransac: " << "\n" << initialTransform << "\n"); ROS_INFO("Extracting corresponding indices"); //int j = 1; indicesSource.clear(); indicesTarget.clear(); for(std::vector<pcl_tutorial::match>::const_iterator iterator_ = fm->matches.begin(); iterator_ != fm->matches.end(); ++iterator_) { indicesSource.push_back(iterator_->trainId); indicesTarget.push_back(iterator_->queryId); // ROS_INFO_STREAM("source point " << j << ": " << featureCloudSource->points[iterator_->queryId].x << ", " << featureCloudSource->points[iterator_->queryId].y << ", " << featureCloudSource->points[iterator_->queryId].z); // ROS_INFO_STREAM("target point " << j++ << ": " << featureCloudTarget->points[iterator_->trainId].x << ", " << featureCloudTarget->points[iterator_->trainId].y << ", " << featureCloudTarget->points[iterator_->trainId].z); // ROS_INFO("qidx: %d tidx: %d iidx: %d dist: %f", iterator_->queryId, iterator_->trainId, iterator_->imgId, iterator_->distance); } /*for (size_t cloudId = 0; cloudId < featureCloudSource->points.size (); ++cloudId) { ROS_INFO_STREAM("feature cloud: " << cloudId << ": " << featureCloudSource->points[cloudId].x << "; " << featureCloudSource->points[cloudId].y << "; " << featureCloudSource->points[cloudId].z); }*/ Eigen::Matrix4f ransacInverse = initialTransform.inverse(); transformPointCloud (*cloud_source, *cloud_ransac_estimation, ransacInverse); ROS_INFO("Writing point clouds"); filename << "cloud" << i << "DenseSource.pcd"; writer.write (filename.str(), *cloud_source, false); filename.str(""); filename << "cloud" << i << "DenseTarget.pcd"; writer.write (filename.str(), *cloud_target, true); filename.str(""); filename << "cloud" << i << "RANSAC-" << (int)indicesSource.size() << "-inliers.pcd"; writer.write (filename.str(), *cloud_ransac_estimation, true); filename.str(""); filename << "cloud" << i << "SparseSource.pcd"; writer.write (filename.str(), *featureCloudSource, false); filename.str(""); filename << "cloud" << i << "SparseTarget.pcd"; writer.write (filename.str(), *featureCloudTarget, false); filename.str(""); i++; // for(std::vector<int>::iterator iterator_ = indicesSource.begin(); iterator_ != indicesSource.end(); ++iterator_) { // ROS_INFO("source indice: %d", *iterator_); // } } else i++; } bag.close(); }
Transform Transform::inverse() const { Eigen::Matrix4f m = util3d::transformToEigen4f(*this); return util3d::transformFromEigen4f(m.inverse()); }
/** \brief Align a pair of PointCloud datasets and return the result * \param cloud_src the source PointCloud * \param cloud_tgt the target PointCloud * \param output the resultant aligned source PointCloud * \param final_transform the resultant transform between source and target */ void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) { // // Downsample for consistency and speed // \note enable this for large datasets PointCloud::Ptr src (new PointCloud); PointCloud::Ptr tgt (new PointCloud); pcl::VoxelGrid<PointT> grid; if (downsample) { grid.setLeafSize (0.05, 0.05, 0.05); grid.setInputCloud (cloud_src); grid.filter (*src); grid.setInputCloud (cloud_tgt); grid.filter (*tgt); } else { src = cloud_src; tgt = cloud_tgt; } // Compute surface normals and curvature PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals); PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals); pcl::NormalEstimation<PointT, PointNormalT> norm_est; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); norm_est.setSearchMethod (tree); norm_est.setKSearch (30); norm_est.setInputCloud (src); norm_est.compute (*points_with_normals_src); pcl::copyPointCloud (*src, *points_with_normals_src); norm_est.setInputCloud (tgt); norm_est.compute (*points_with_normals_tgt); pcl::copyPointCloud (*tgt, *points_with_normals_tgt); // // Instantiate our custom point representation (defined above) ... MyPointRepresentation point_representation; // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z float alpha[4] = {1.0, 1.0, 1.0, 1.0}; point_representation.setRescaleValues (alpha); // // Align pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; reg.setTransformationEpsilon (1e-6); // Set the maximum distance between two correspondences (src<->tgt) to 10cm // Note: adjust this based on the size of your datasets reg.setMaxCorrespondenceDistance (0.1); // Set the point representation reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation)); reg.setInputSource (points_with_normals_src); reg.setInputTarget (points_with_normals_tgt); // // Run the same optimization in a loop and visualize the results Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource; PointCloudWithNormals::Ptr reg_result = points_with_normals_src; reg.setMaximumIterations (2); for (int i = 0; i < 30; ++i) { PCL_INFO ("Iteration Nr. %d.\n", i); // save cloud for visualization purpose points_with_normals_src = reg_result; // Estimate reg.setInputSource (points_with_normals_src); reg.align (*reg_result); //accumulate transformation between each Iteration Ti = reg.getFinalTransformation () * Ti; //if the difference between this transformation and the previous one //is smaller than the threshold, refine the process by reducing //the maximal correspondence distance if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ()) reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001); prev = reg.getLastIncrementalTransformation (); // visualize current state showCloudsRight(points_with_normals_tgt, points_with_normals_src); } // // Get the transformation from target to source targetToSource = Ti.inverse(); // // Transform target back in source frame pcl::transformPointCloud (*cloud_tgt, *output, targetToSource); p->removePointCloud ("source"); p->removePointCloud ("target"); PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0); PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0); p->addPointCloud (output, cloud_tgt_h, "target", vp_2); p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2); PCL_INFO ("Press q to continue the registration.\n"); p->spin (); p->removePointCloud ("source"); p->removePointCloud ("target"); //add the source to the transformed target *output += *cloud_src; final_transform = targetToSource; }
Transformation * UpdateWeightFilterv3::filterTransformation(Transformation * input){ struct timeval start, end; gettimeofday(&start, NULL); RGBDFrame * src = input->src; RGBDFrame * dst = input->dst; FrameInput * src_fi = src->input; FrameInput * dst_fi = dst->input; Transformation * transformation = input->clone(); /* Transformation * transformation = new Transformation(); transformation->transformationMatrix = input->transformationMatrix; transformation->src = src; transformation->dst = dst; transformation->weight = input->weight;//0; for(int i = 0; i < input->matches.size();i++){ KeyPoint * src_kp = input->matches.at(i).first; KeyPoint * dst_kp = input->matches.at(i).second; transformation->matches.push_back(make_pair(src_kp, dst_kp)); } */ Eigen::Matrix4f transformationMat = input->transformationMatrix; Eigen::Matrix4f transformationMat_inv = transformationMat.inverse(); float counter_good = 0; float counter_bad = 0; float counter_total = 0; float d1[dst->validation_points.size()]; int nr_valid = 0; src_fi->getDiff(transformationMat_inv , dst->validation_points , d1, nr_valid ); for(int i = 0; i < nr_valid; i++){ float diff = d1[i]; if(fabs(diff) < limit) {counter_good++;} counter_total++; } float d2[src->validation_points.size()]; nr_valid = 0; dst_fi->getDiff(transformationMat , src->validation_points , d2, nr_valid ); for(int i = 0; i < nr_valid; i++){ float diff = d2[i]; if(fabs(diff) < limit) {counter_good++;} counter_total++; } if(counter_total < min_points){counter_total = min_points;} transformation->weight = 10 + (counter_good-bad_weight*counter_bad)/counter_total; //printf("input w: %f update weight results: %f %f %f-> %f\n",input->weight,counter_good,counter_bad,counter_total,transformation->weight); gettimeofday(&end, NULL); float time = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f; //printf("UpdateWeightFilterv3 cost: %f\n",time); return transformation; }