Ошибка сегментации при выполнении настройки пучка с помощью g2o

Question

void bundleAdjustment ( const vector< cv::Point3f > points_3d,
                         const vector< cv::Point2f > points_2d, 
                         const Mat& K, Mat& R, Mat& t )
{
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparseBlock::PoseMatrixType();
Block* solver_ptr = new Block ( linearSolver );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );

g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (R_mat,
Eigen::Vector3d ( t.at ( 0,0 ), t.at ( 1,0 ), t.at ( 2,0 ) ) ) );
optimizer.addVertex ( pose );

int index = 1;
for (const  Point3f p:points_3d )   // landmarks, world coordinates.
{
    g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
    point->setId ( index++ ); 
    point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
    point->setMarginalized ( true ); 
   optimizer.addVertex ( point );
}

g2o::CameraParameters* camera = new g2o::CameraParameters (K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
camera->setId ( 0 );
optimizer.addParameter ( camera ); //Add camera parameters

index = 1;
for ( const Point2f p:points_2d ) //Add edges(observed data)
{
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId ( index );
    edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
    edge->setVertex ( 1, pose );
    edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
    edge->setParameterId ( 0,0 );  //cameraparameter
    edge->setInformation ( Eigen::Matrix2d::Identity() );
    optimizer.addEdge ( edge );
    index++;
}

chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;


cout<<endl<<"after optimization:"<<endl;
cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;

}

Я определил функцию для выполнения настройки связки с помощью g2o, но терминал подтвердил ошибку сегментации, когда я запустил исполняемый файл. Я уверен, что проблема связана с этой функцией, потому что вся программа может работать должным образом после закомментирования этой функции.

Моя платформа Ubuntu16.04 LTS.

EDIT1: я пытался добавить set( CMAKE_BUILD_TYPE "Release" ) в моем CMakeLists.txt. Затем терминал подтвердил двойное освобождение или повреждение (выход) после запуска исполняемого файла. Тем не менее, он работал правильно после того, как я закомментировал следующий блок кода:

g2o::CameraParameters* camera = new g2o::CameraParameters (K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );  

РЕДАКТИРОВАТЬ 2: Вся программа может работать правильно, как только я использовал конструктор по умолчанию CameraParameter:

g2o::CameraParameters* camera = new g2o::CameraParameters ();
        camera->setId ( 0 );
        optimizer.addParameter ( camera );  

Однако ошибка сегментации существовала до сих пор.

РЕДАКТИРОВАТЬ 3: После закомментирования следующих кодов больше не происходит ошибок сегментации:

optimizer.addVertex ( pose );
 optimizer.addVertex ( point );
 optimizer.addEdge ( edge );
 optimizer.addParameter ( camera )