Camera* Camera::getDefaultCamera() { auto scene = Director::getInstance()->getRunningScene(); if(scene) { return scene->getDefaultCamera(); } return nullptr; }
void visualOdometry::mainloop(){ FRAME lastFrame; std::string file_name_ground =pd_.getData( "file_name_ground"); readFromFile( file_name_ground,ground_truth_list_); lastFrame = readFrameFromGroundTruthList(start_index_ ,ground_truth_list_); // 我们总是在比较currFrame和lastFrame string detector = pd_.getData( "detector" ); string descriptor = pd_.getData( "descriptor" ); CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); computeKeyPointsAndDesp( lastFrame, detector, descriptor ); // 是否显示点云 bool visualize = pd_.getData("visualize_pointcloud")==string("true"); int min_inliers = atoi( pd_.getData("min_inliers").c_str() ); double max_norm = atof( pd_.getData("max_norm").c_str() ); for (int currIndex=start_index_+1; currIndex<end_index_; currIndex++ ) { current_index_ = currIndex; cout<<"Reading files "<<currIndex<<endl; FRAME currFrame = readFrameFromGroundTruthList(currIndex, ground_truth_list_); // 读取currFrame computeKeyPointsAndDesp( currFrame, detector, descriptor ); // 比较currFrame 和 lastFrame RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera ); if ( result.inliers < min_inliers ){ //inliers不够,放弃该帧 std::cout<<"not enouth inliers: "<<result.inliers; continue; } //ROS_INFO_STREAM("trans: "<< result.tvec <<" rot: "<< result.rvec); cv::Mat rotation_matrix; cv::Rodrigues(result.rvec, rotation_matrix); Eigen::Matrix3d rotation_eigen_temp; cv::cv2eigen(rotation_matrix,rotation_eigen_temp); Eigen::AngleAxisd rotation_eigen(rotation_eigen_temp); Eigen::Affine3d incrementalAffine = Eigen::Affine3d::Identity(); incrementalAffine = rotation_eigen; incrementalAffine(0,3) = result.tvec.ptr<double>(0)[0]; incrementalAffine(1,3) = result.tvec.ptr<double>(1)[0]; incrementalAffine(2,3) = result.tvec.ptr<double>(2)[0]; //pose_camera_ = incrementalAffine * pose_camera_; publishTrajectory(); publishGroundTruth(); ROS_INFO_STREAM("RT: "<< incrementalAffine.matrix()); lastFrame = currFrame; } }
//reset_view_button_ = NULL; reset_view_button_->hide(); // make sure we're still able to cancel set goal pose QObject::connect(render_panel_, SIGNAL(signalKeyPressEvent(QKeyEvent*)), this, SLOT(keyPressEvent(QKeyEvent*))); QObject::connect(render_panel_, SIGNAL(signalMouseEnterEvent(QEvent*)), this, SLOT(mouseEnterEvent(QEvent*))); QObject::connect(render_panel_, SIGNAL(signalMouseMoveEvent(QMouseEvent*)), this, SLOT(mouseMoveEvent(QMouseEvent*))); Q_FOREACH( QWidget* sp, findChildren<QWidget*>() ) { sp->installEventFilter( this ); sp->setMouseTracking( true ); } // Set image topic int default_cam = getDefaultCamera(); /* camera_viewer_->subProp( "Image Topic" )->setValue( (camera_[default_cam].topic_prefix+"_full/image_raw").c_str() ); camera_viewer_->subProp( "Image Request Topic" )->setValue( (camera_[default_cam].topic_prefix+"_full/image_request").c_str() ); camera_viewer_->subProp( "Cropped Image Topic" )->setValue( (camera_[default_cam].topic_prefix+"_cropped/image_raw").c_str() ); camera_viewer_->subProp( "Cropped Image Request Topic" )->setValue( (camera_[default_cam].topic_prefix+"_cropped/image_request").c_str() ); */ this->changeCameraTopic(default_cam); camera_viewer_->setEnabled(false); camera_viewer_->setEnabled(true); selected_area_[0] = 0; selected_area_[1] = 0; selected_area_[2] = 0;