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;
