[g2o]一个备忘

void Scan::Transform(Scan *newScan,const Eigen::Isometry3d& pose)
{
int count=ml_pts->size();
m_model=MatrixXd::Zero(4,count);
int nidx=0;
for (vector<PointXYZ>::iterator iter = ml_pts->begin();iter< ml_pts->end();iter++) //iter != modelslist->end(); ++iter)
{
PointXYZ tmp=(PointXYZ)*iter;
m_model(0,nidx)=tmp.X;
m_model(1,nidx)=tmp.Y;
m_model(2,nidx)=tmp.Z;
m_model(3,nidx)=1;
nidx++;
}
m_model=pose.matrix()*m_model;

}