Greetings Luiz.
First some info about the data. ISfmReconstruction contains aligned camera positions and raw sfm point cloud. By creating
an ISceneStructure one obtains access, for each camera, to explicit list of sfm points which it can see (or was used to calculate them). Note that if You do not need this decomposition, it is faster to access the points (and cameras) directly from ISfmReconstruction.
Schematic ISceneStructure usage example.
- Code:
ISceneStructure::GetCameraReferences( &pCameraReferences ) // get info structures for all cameras
CameraReference myCamera = pCameraReferences[myCameraIndex] // select the camera I am in love with
ISceneStructure::GetPointsIndexes( &pCameraPointsLists ) // obtain explicit point list for each camera
ISceneStructure::GetPoints( &pPoints ) // also obtain the actual sfm points
myCameraPointIndexList = pCameraPointsLists[myCamera.pointIndex] // select list for my camera
for ( i = 0; i < myCamera.pointCount; i++ )
pPoints[myCameraPointIndexList[ i]] // access all sfm points seen by my beloved camera
To Your actual question. From technical reasons, the sfm point cloud and the model triangulation can be in different coordinate systems. In order to work with their geometries together, it is necessary to transform them into the same space.
RootCoordinateSystem - somewhere at the beginning 
ActualSfmCoordinateSystem - here are the points from ISfmReconstruction
PartialModelCoordinateSystem - here is the model triangulation (in this case it is not important why it is only partial)
Schematic example of transforming vertex from model space into sfm point space.
- Code:
CoordinateSystemResidual csRoot2Sfm = ISfmReconstruction->GetResidualTransform()
CoordinateSystemResidual csRoot2Model = IMvsModel->GetResidualTransform()
CoordinateSystemResidual csModel2Root = InvertResidualTransform( &csRoot2Model )
CoordinateSystemResidual csModel2Sfm = TransformResidualTransform( &csRoot2Sfm, &csModel2Root )
// now we can transform vertices from model space into actual sfm space
vertexInSfmSpace = CoordinateSystemPointTransform( csModel2Sfm, vertexInModelSpace )
Schemes for helper functions. Low-level mathematical functions are omitted.
- Code:
CoordinateSystemResidual InvertResidualTransform( __in const CoordinateSystemResidual* pTransform )
CoordinateSystemResidual inverted
// rotation matrix
inverted.R = TransposeMatrix33( pTransform ->R )
// scale factor
inverted.s = 1.0 / pTransform ->s
// translation vector
double t[3] = MultiplyMatrix33Vector3( pInverted->R, pTransform->t )
inverted.t[0] = - inverted.s * t[0];
inverted.t[1] = - inverted.s * t[1];
inverted.t[2] = - inverted.s * t[2];
- Code:
CoordinateSystemResidual TransformResidualTransform(
__in const CoordinateSystemResidual* pA,
__ in const CoordinateSystemResidual* pB )
CoordinateSystemResidual AB
// rotation matrix
AB.R = MultiplyMatrix33Matrix33( pA->R, pB->R )
// scale
AB.s = pA->s * pB->s
// translation vector
double t[3] = MultiplyMatrix33Vector3( pA->R, pB->t )
AB.t[0] = pA->t[0] + pA->s * t[0]
AB.t[1] = pA->t[1] + pA->s * t[1]
AB.t[2] = pA->t[2] + pA->s * t[2]
return AB
- Code:
... CoordinateSystemPointTransform(...)
// transform a vector with rotation matrix and translation vector
v' = R * v + t