Exemplo n.º 1
0
ENGINE_NAMESPACE_BEGIN_MOVIESYSTEM
//-------------------------------------------------------------------------------------------------
void sdAnimUIntTrack::GetValue(float fTime, uint& uiValue) 
{
	if (GetNumKeys() == 0)
	{
		uiValue = (uint)-1;
		return;
	}
	else if (GetNumKeys() == 1)
	{
		uiValue = GetKeyTime(0) <= fTime ? m_kTracks[0].m_uiValue : (uint)-1;
		return;
	}
	else
	{
		for (int i = 1; i < GetNumKeys(); ++i)
		{
			if (fTime >= GetKeyTime(i-1) && fTime < GetKeyTime(i))
			{
				uiValue = m_kTracks[i-1].m_uiValue;
				return;
			}
		}

		uiValue = m_kTracks[GetNumKeys() - 1].m_uiValue;
	}
}
Exemplo n.º 2
0
void FIndexedCurve::EnsureAllIndicesHaveHandles() const
{
	if (KeyHandlesToIndices.Num() != GetNumKeys())
	{
		for (int32 i = 0; i < GetNumKeys(); ++i)
		{
			EnsureIndexHasAHandle(i);
		}
	}
}
Exemplo n.º 3
0
FKeyHandle FIndexedCurve::GetKeyHandle(int32 KeyIndex) const
{
	check(KeyIndex >= 0 && KeyIndex < GetNumKeys());
	EnsureIndexHasAHandle(KeyIndex);

	return *KeyHandlesToIndices.FindKey(KeyIndex);
}
Exemplo n.º 4
0
//-------------------------------------------------------------------------------------------------
void sdAnimUIntTrack::SetValue(int iIndex, uint uiValue) 
{
	if (iIndex > 0 && iIndex < GetNumKeys())
	{
		m_kTracks[iIndex].m_uiValue = uiValue;
	}
}
Exemplo n.º 5
0
//-------------------------------------------------------------------------------------------------
bool sdAnimUIntTrack::Save(Engine::Util::sdLuaWriteUtil& kLuaStream)
{
	const int iKeyLen = strlen("                ");

	kLuaStream.WriteNodeBegin("Track");
	kLuaStream.WriteData("TrackType", "ATRACK_UINT", iKeyLen);
	{
		kLuaStream.WriteData("TrackFlag", m_iFlags, iKeyLen);
		kLuaStream.WriteNodeBegin("Value");

		for (int i = 0; i < GetNumKeys(); ++i)
		{
			sdUIntKey kKey;
			GetKey(i, &kKey);

			kLuaStream.LoopInnerBegin();
			kLuaStream.WriteData("Time",  kKey.m_fTime, iKeyLen);
			kLuaStream.WriteData("Flags", kKey.m_iFlags,iKeyLen);
			kLuaStream.WriteData("UInt",  kKey.m_uiValue,iKeyLen);
			kLuaStream.LoopInnerEnd();
		}

		kLuaStream.WriteNodeEnd();
	}
	kLuaStream.WriteNodeEnd();

	return true;
}
Exemplo n.º 6
0
//-------------------------------------------------------------------------------------------------
void sdAnimUIntTrack::GetValue(int iIndex, uint& uiValue) 
{
	if (iIndex < 0 || iIndex >= GetNumKeys())
	{
		uiValue = (uint)-1;
		return;
	}
	else
	{
		uiValue = m_kTracks[iIndex].m_uiValue;
		return;
	}
}
Exemplo n.º 7
0
// --[  Method  ]---------------------------------------------------------------
//
//  - Class     : CSplineTCB
//  - Prototype : void NotifyKeyValueChanged(int nIndex)
//
//  - Purpose   : This method is called when a key's T/C/B value(s) has/have
//                been changed in order to recompute its tangents.
//
// -----------------------------------------------------------------------------
void CSplineTCB::NotifyKeyTCBChanged(int nIndex)
{
	assert(ValidKeyIndex(nIndex));

	if(GetNumKeys() < 2)
	{
		return;
	}

	int nPrevious = nIndex - 1, nNext = nIndex + 1;

	if(nIndex == 0)
	{
		nPrevious = 0;
	}

	if(nIndex == GetNumKeys() - 1)
	{
		nNext = GetNumKeys() - 1;
	}

	GetTangents(m_vecKeys[nPrevious], m_vecTCBKeys[nIndex], m_vecKeys[nNext], &m_vecKeys[nIndex].v3TanIn, &m_vecKeys[nIndex].v3TanOut);
}
Exemplo n.º 8
0
// --[  Method  ]---------------------------------------------------------------
//
//  - Class     : CSplineTCB
//  - Prototype : void NotifyKeyValueChanged(int nIndex)
//
//  - Purpose   : This method is called when a key value has been changed in
//                order to recompute its neighbour tangent values.
//                Special cases are treated as explained in AddKey().
//
// -----------------------------------------------------------------------------
void CSplineTCB::NotifyKeyValueChanged(int nIndex)
{
	// Adjust previous key's tangents if there's one

	if(nIndex > 0)
	{
		// Is key[nIndex-1] the first? Duplicate then, otherwise no need to duplicate.

		if(nIndex == 1)
		{
			GetTangents(m_vecKeys[0], m_vecTCBKeys[0], m_vecKeys[nIndex], &m_vecKeys[0].v3TanIn, &m_vecKeys[0].v3TanOut);
		}
		else
		{
			GetTangents(m_vecKeys[nIndex - 2], m_vecTCBKeys[nIndex - 1], m_vecKeys[nIndex], &m_vecKeys[nIndex - 1].v3TanIn, &m_vecKeys[nIndex - 1].v3TanOut);
		}
	}

	// Adjust next key's tangents if there's one

	if(nIndex < GetNumKeys() - 1)
	{
		// Is key[nIndex+1] the last? Duplicate then, otherwise no need to duplicate.

		int nLast = GetNumKeys() - 1;

		if(nIndex + 1 == nLast)
		{
			GetTangents(m_vecKeys[nIndex], m_vecTCBKeys[nLast], m_vecKeys[nLast], &m_vecKeys[nLast].v3TanIn, &m_vecKeys[nLast].v3TanOut);
		}
		else
		{
			GetTangents(m_vecKeys[nIndex], m_vecTCBKeys[nIndex + 1], m_vecKeys[nIndex + 2], &m_vecKeys[nIndex + 1].v3TanIn, &m_vecKeys[nIndex + 1].v3TanOut);
		}
	}
}
Exemplo n.º 9
0
//-------------------------------------------------------------------------------------------------
void sdAnimUIntTrack::SetValue(float fTime, uint uiValue) 
{
	sdUIntKey kKey(fTime, IKey::E_KEY_FLAG_CONSTANT, uiValue);
	int iIndex = FindKey(fTime);
	if (iIndex == -1)
	{
		int iKeyNum = GetNumKeys();
		SetNumKeys(iKeyNum + 1);
		SetKey(iKeyNum, &kKey);
		SortKeys();
	}
	else
	{
		SetKey(iIndex, &kKey);
	}
}
Exemplo n.º 10
0
LWEnvelopeID LWEnvelope::CopyToGroup(LWEnvelopeID result)
{
LWEnvKeyframeID curKey=0;
LWEnvKeyframeID createdKey=0;
double dValue;
double dTime;
int iValue;

NumKeys=GetNumKeys();

for ( int i=0; i<NumKeys; i++)
{
	curKey=Lightwave3D::envfunc->nextKey(Envelope,curKey);
	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_TIME,&dTime);
	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_VALUE,&dValue);

	createdKey=Lightwave3D::envfunc->createKey(result,dTime,dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_SHAPE,&iValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_SHAPE,&iValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_TENSION,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_TENSION,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_BIAS,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_BIAS,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_CONTINUITY,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_CONTINUITY,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_PARAM_0,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_PARAM_0,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_PARAM_1,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_PARAM_1,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_PARAM_2,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_PARAM_2,&dValue);

	Lightwave3D::envfunc->keyGet(Envelope, curKey,LWKEY_PARAM_3,&dValue);
	Lightwave3D::envfunc->keySet(result, createdKey,LWKEY_PARAM_3,&dValue);

}

	return result;
}
Exemplo n.º 11
0
const char *GetKeyString(entity_t *ent, int iIndex)
{
//	for (epair_t* ep=ent->epairs ; ep ; ep=ep->next)
//	{
//		if (!iIndex--)
//			return ep->key;
//	}
//
//	assert(0);
//	return NULL;

    if ( iIndex < GetNumKeys(ent) )
    {
        return ent->epairs.GetKeyVal(iIndex)->GetKey().c_str();
    }

    assert(0);
    return NULL;
}
Exemplo n.º 12
0
// --[  Method  ]---------------------------------------------------------------
//
//  - Class     : CSplineTCB
//  - Prototype : bool AddKey(const TKey& tcbKey)
//
//  - Purpose   : Adds a key to the set. Operates like CSplineTCB::AddKey()
//                but we make some operations with tension, bias and continuity
//                to compute the tangents. We also need p-1 and p+1's CVector3
//                values.
//
//  - Note      : Special cases first and last key are dealed duplicating
//                these keys (remember, we need p-1 and p+1). Duplicating means
//                that we consider the previous key to the first as the
//                same and also the next to the last as the last itself.
//                We also need to keep a secondary TCB key list to recompute
//                tangents of the neighbour keys when a key is changed or a new
//                one is inserted.
//
// -----------------------------------------------------------------------------
bool CSplineTCB::AddKey(const TKey& tcbKey)
{
	CSpline::TKey newKey;

	newKey.fTime   = tcbKey.fTime;
	newKey.v3Value = tcbKey.v3Value;

	// Secuencial search for a t greater than tcbKey.time:

	std::vector<CSpline::TKey>::iterator it;

	for(it = m_vecKeys.begin(); it != m_vecKeys.end(); ++it)
	{
		if(it->fTime > tcbKey.fTime)
		{
			// We know there's at least a key after the one being inserted
			// (key.time < keylist[i].time), so the insertion can be only
			// at the list's start or between two keys.

			int nIndex, nLast;
			
			nIndex = it - m_vecKeys.begin();
			nLast  = GetNumKeys() - 1;

			if(nIndex == 0)
			{
				// Added as the first key. Duplicate it to compute its tangents.
				GetTangents(newKey, tcbKey, m_vecKeys.front(), &newKey.v3TanIn, &newKey.v3TanOut);
			}
			else
			{
				// Added in between two keys:
				GetTangents(m_vecKeys[nIndex - 1], tcbKey, m_vecKeys[nIndex], &newKey.v3TanIn, &newKey.v3TanOut);
			}

			m_vecKeys.insert(it, newKey);
			m_vecTCBKeys.insert(m_vecTCBKeys.begin() + (it - m_vecKeys.begin()), tcbKey);

			NotifyKeyValueChanged(nIndex);
			return true;
		}
	}

	// List is empty?

	if(m_vecKeys.empty())
	{
		m_vecKeys.push_back(newKey);
		m_vecTCBKeys.push_back(tcbKey);
		return true;
	}

	// Added at the end of the list. Duplicate it to compute its tangents.

	GetTangents(m_vecKeys.back(), tcbKey, newKey, &newKey.v3TanIn, &newKey.v3TanOut);
	m_vecKeys.push_back(newKey);
	m_vecTCBKeys.push_back(tcbKey);

	NotifyKeyValueChanged(GetNumKeys() - 1);

	return true;
}
Exemplo n.º 13
0
/* Add new points to the bundle adjustment */
int 
BundlerApp::BundleAdjustAddAllNewPoints(int num_points, int num_cameras,
                                        int *added_order,
                                        camera_params_t *cameras,
                                        v3_t *points, v3_t *colors,
                                        double reference_baseline,
                                        std::vector<ImageKeyVector> &pt_views,
                                        double max_reprojection_error,
                                        int min_views)
{
    std::vector<int> track_idxs;
    std::vector<ImageKeyVector> new_tracks;

    int num_tracks_total = (int) m_track_data.size();
    int *tracks_seen = new int[num_tracks_total];
    for (int i = 0; i < num_tracks_total; i++) {
	tracks_seen[i] = -1;
    }

    /* Gather up the projections of all the new tracks */
    for (int i = 0; i < num_cameras; i++) {
	int image_idx1 = added_order[i];

	int num_keys = GetNumKeys(image_idx1);
	
	for (int j = 0; j < num_keys; j++) {
	    Keypoint &key = GetKey(image_idx1, j);

	    if (key.m_track == -1)
		continue;  /* Key belongs to no track */

	    if (key.m_extra != -1)
		continue;  /* Key is outlier or has already been added */

	    int track_idx = key.m_track;
	    
	    /* Check if this track is already associated with a point */
	    if (m_track_data[track_idx].m_extra != -1)
		continue;

	    /* Check if we've seen this track */
	    int seen = tracks_seen[track_idx];

	    if (seen == -1) {
		/* We haven't yet seen this track, create a new track */
		tracks_seen[track_idx] = (int) new_tracks.size();

		ImageKeyVector track;
		track.push_back(ImageKey(i, j));
		new_tracks.push_back(track);
		track_idxs.push_back(track_idx);
	    } else {
		new_tracks[seen].push_back(ImageKey(i, j));
	    }
	}
    }
    
    delete [] tracks_seen;

    /* Now for each (sub) track, triangulate to see if the track is
     * consistent */
    int pt_count = num_points;

    int num_ill_conditioned = 0;
    int num_high_reprojection = 0;
    int num_cheirality_failed = 0;
    int num_added = 0;

    int num_tracks = (int) new_tracks.size();
    for (int i = 0; i < num_tracks; i++) {
	int num_views = (int) new_tracks[i].size();
	
	if (num_views < min_views) continue;  /* Not enough views */

#if 0
	printf("Triangulating track ");
	PrintTrack(new_tracks[i]);
	printf("\n");
#endif

	/* Check if at least two cameras fix the position of the point */
	bool conditioned = false;
	bool good_distance = false;
	double max_angle = 0.0;
	for (int j = 0; j < num_views; j++) {
	    for (int k = j+1; k < num_views; k++) {
		int camera_idx1 = new_tracks[i][j].first;
		int image_idx1 = added_order[camera_idx1];
		int key_idx1 = new_tracks[i][j].second;

		int camera_idx2 = new_tracks[i][k].first;
		int image_idx2 = added_order[camera_idx2];
		int key_idx2 = new_tracks[i][k].second;

		Keypoint &key1 = GetKey(image_idx1, key_idx1);
		Keypoint &key2 = GetKey(image_idx2, key_idx2);

		v2_t p = v2_new(key1.m_x, key1.m_y);
		v2_t q = v2_new(key2.m_x, key2.m_y);

                if (m_optimize_for_fisheye) {
                    double p_x = Vx(p), p_y = Vy(p);
                    double q_x = Vx(q), q_y = Vy(q);
                    
                    m_image_data[image_idx1].
                        UndistortPoint(p_x, p_y, Vx(p), Vy(p));
                    m_image_data[image_idx2].
                        UndistortPoint(q_x, q_y, Vx(q), Vy(q));
                }

		double angle = ComputeRayAngle(p, q, 
					       cameras[camera_idx1], 
					       cameras[camera_idx2]);

		if (angle > max_angle)
		    max_angle = angle;

		/* Check that the angle between the rays is large
		 * enough */
		if (RAD2DEG(angle) >= m_ray_angle_threshold) {
		    conditioned = true;
		}

#if 0
		double dist_jk = 
		    GetCameraDistance(cameras + j, cameras + k, 
				      m_explicit_camera_centers);

		if (dist_jk > m_min_camera_distance_ratio * reference_baseline)
		    good_distance = true;
#else
                good_distance = true;
#endif
	    }
	}
	
	if (!conditioned || !good_distance) {
	    num_ill_conditioned++;

#if 0
	    printf(">> Track is ill-conditioned [max_angle = %0.3f]\n", 
		   RAD2DEG(max_angle));
	    fflush(stdout);
#endif
	    continue;
	}
	
	double error;
	v3_t pt;

        if (!m_panorama_mode) {
            pt = TriangulateNViews(new_tracks[i], added_order, cameras, 
                                   error, true);
        } else {
            pt = GeneratePointAtInfinity(new_tracks[i], added_order, cameras, 
                                         error, true);
        }
       
		// Changed by Wan, Yi
	if (::isnan(error) || error > max_reprojection_error) {
	    num_high_reprojection++;
#if 0
	    printf(">> Reprojection error [%0.3f] is too large\n", error);
	    fflush(stdout);
#endif
	    continue;	    
	}

	bool all_in_front = true;
	for (int j = 0; j < num_views; j++) {
	    int camera_idx = new_tracks[i][j].first;
	    bool in_front = CheckCheirality(pt, cameras[camera_idx]);
	 
	    if (!in_front) {
		all_in_front = false;
		break;
	    }
	}

	if (!all_in_front) {
	    num_cheirality_failed++;

#if 0
	    printf(">> Cheirality check failed\n");
	    fflush(stdout);
#endif
	    continue;
	}
	
	/* All tests succeeded, so let's add the point */
#if 0
	printf("Triangulating track ");
	PrintTrack(new_tracks[i]);
	printf("\n");
	printf(">> All tests succeeded [%0.3f, %0.3f] for point [%d]\n", 
	       RAD2DEG(max_angle), error, pt_count);
#endif

	fflush(stdout);

	points[pt_count] = pt;

	int camera_idx = new_tracks[i][0].first;
	int image_idx = added_order[camera_idx];
	int key_idx = new_tracks[i][0].second;

	unsigned char r = GetKey(image_idx, key_idx).m_r;
	unsigned char g = GetKey(image_idx, key_idx).m_g;
	unsigned char b = GetKey(image_idx, key_idx).m_b;
	colors[pt_count] = v3_new((double) r, (double) g, (double) b);
    
	pt_views.push_back(new_tracks[i]);

	/* Set the point index on the keys */
	for (int j = 0; j < num_views; j++) {
	    int camera_idx = new_tracks[i][j].first;
	    int image_idx = added_order[camera_idx];
	    int key_idx = new_tracks[i][j].second;
	    GetKey(image_idx, key_idx).m_extra = pt_count;
	}

	int track_idx = track_idxs[i];
	m_track_data[track_idx].m_extra = pt_count;
	
	pt_count++;
        num_added++;
    }

    printf("[AddAllNewPoints] Added %d new points\n", num_added);
    printf("[AddAllNewPoints] Ill-conditioned tracks: %d\n", 
           num_ill_conditioned);
    printf("[AddAllNewPoints] Bad reprojections: %d\n", num_high_reprojection);
    printf("[AddAllNewPoints] Failed cheirality checks: %d\n", 
           num_cheirality_failed);

    return pt_count;
}
Exemplo n.º 14
0
bool LWEnvelope::UpdateKeyTable()
{
NumKeys=GetNumKeys();

Keys.clear();

if (NumKeys<=0)
{
	StartTime=0.0;
	EndTime=0.0;
	TimeSpan=0.0;
	return false;
}

LWEnvKeyframeID curKey=Lightwave3D::envfunc->nextKey(Envelope, NULL);
	
for (int i=0;i<NumKeys;i++)
{
LWKey tempKey;

		Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_TIME, &tempKey.Time );
		Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_VALUE, &tempKey.value );
		Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_SHAPE, &tempKey.shape );


   switch( tempKey.shape ) {
     case 0:        // TCB
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_TENSION, &tempKey.tension );
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_CONTINUITY, &tempKey.continuity );
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_BIAS, &tempKey.bias );

       break;

     case 1:        // Hermite
     case 2:        // Bezier
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_PARAM_0, &tempKey.par0 );
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_PARAM_1, &tempKey.par1 );
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_PARAM_2, &tempKey.par2 );
       Lightwave3D::envfunc->keyGet( Envelope, curKey, LWKEY_PARAM_3, &tempKey.par3 );

       break;

     case 3:        // Linear
       break;

     case 4:        // Stepped
       break;

	 case 5:
		 break;     // 2DBezier   FIX IT!!
	 }
   Keys.push_back(tempKey);
		curKey=Lightwave3D::envfunc->nextKey(Envelope,curKey);
}

StartTime=Keys.front().Time;
EndTime=Keys.back().Time;
TimeSpan=EndTime-StartTime;
index=0;
Lightwave3D::envfunc->egGet( Envelope, Lightwave3D::chaninfo->channelParent(EnvChannel),LWENVTAG_PREBEHAVE, &PrePostBehavior[0]);
Lightwave3D::envfunc->egGet( Envelope, Lightwave3D::chaninfo->channelParent(EnvChannel),LWENVTAG_POSTBEHAVE, &PrePostBehavior[1]);

return true;
}
Exemplo n.º 15
0
/* Dump an output file containing information about the current
* state of the world */
void BaseApp::DumpOutputFile(const char *output_dir, const char *filename, 
                             int num_images, int num_cameras, int num_points,
                             int *added_order, 
                             camera_params_t *cameras, 
                             v3_t *points, v3_t *colors,
                             std::vector<ImageKeyVector> &pt_views
                             /*bool output_radial_distortion*/)
{
    clock_t start = clock();

    int num_visible_points = 0;
    
    for (int i = 0; i < num_points; i++) {
        if (pt_views[i].size() > 0)
            num_visible_points++;
    }

    char buf[256];
    sprintf(buf, "%s/%s", output_dir, filename);

    FILE *f = fopen(buf, "w");
    if (f == NULL) {
        printf("Error opening file %s for writing\n", buf);
        return;
    }

    // if (output_radial_distortion) {
    /* Print version number */
    // fprintf(f, "# Bundle file v0.4\n");
    fprintf(f, "# Bundle file v0.3\n");
    // }

    fprintf(f, "%d %d\n", num_images, num_visible_points);

    /* Dump cameras */
    for (int i = 0; i < num_images; i++) {

#if 0
        /* Print the name of the file */
        fprintf(f, "%s %d %d\n", 
                m_image_data[i].m_name, 
                m_image_data[i].GetWidth(), m_image_data[i].GetHeight());
#endif

        int idx = -1;
        for (int j = 0; j < num_cameras; j++) {
            if (added_order[j] == i) {
                idx = j;
                break;
            }
        }

        if (idx == -1) {
            // if (!output_radial_distortion)
            //     fprintf(f, "0\n");
            // else
            fprintf(f, "0 0 0\n");
            fprintf(f, "0 0 0\n0 0 0\n0 0 0\n0 0 0\n");
        } else {
            // if (!output_radial_distortion)
            //     fprintf(f, "%0.10e\n", cameras[idx].f);
            // else
            fprintf(f, "%0.10e %0.10e %0.10e\n", 
                    cameras[idx].f, cameras[idx].k[0], cameras[idx].k[1]);

            fprintf(f, "%0.10e %0.10e %0.10e\n", 
                cameras[idx].R[0], 
                cameras[idx].R[1], 
                cameras[idx].R[2]);
            fprintf(f, "%0.10e %0.10e %0.10e\n", 
                cameras[idx].R[3], 
                cameras[idx].R[4], 
                cameras[idx].R[5]);
            fprintf(f, "%0.10e %0.10e %0.10e\n", 
                cameras[idx].R[6], 
                cameras[idx].R[7], 
                cameras[idx].R[8]);

            double t[3];
            matrix_product(3, 3, 3, 1, cameras[idx].R, cameras[idx].t, t);
            matrix_scale(3, 1, t, -1.0, t);
            fprintf(f, "%0.10e %0.10e %0.10e\n", t[0], t[1], t[2]);
        }
    }

    /* Dump points */
    for (int i = 0; i < num_points; i++) {
        int num_visible = (int) pt_views[i].size();

        if (num_visible > 0) {

            /* Position */
            fprintf(f, "%0.10e %0.10e %0.10e\n", 
                    Vx(points[i]), Vy(points[i]), Vz(points[i]));
            // Vx(points[idx]), Vy(points[idx]), Vz(points[idx]));

            /* Color */
            fprintf(f, "%d %d %d\n", 
                    iround(Vx(colors[i])), 
                    iround(Vy(colors[i])), 
                    iround(Vz(colors[i])));

            int num_visible = (int) pt_views[i].size();
            fprintf(f, "%d", num_visible);
            for (int j = 0; j < num_visible; j++) {
                int img = added_order[pt_views[i][j].first];
                int key = pt_views[i][j].second;
                
                double x = m_image_data[img].m_keys[key].m_x;
                double y = m_image_data[img].m_keys[key].m_y;
                
                fprintf(f, " %d %d %0.4f %0.4f", img, key, x, y);
            }
            
            fprintf(f, "\n");
        }
    }

#if 0
    /* Finally, dump all outliers */
    ImageKeyVector outliers;
    for (int i = 0; i < num_images; i++) {
        /* Find the index of this camera in the ordering */
        int idx = -1;
        for (int j = 0; j < num_cameras; j++) {
            if (added_order[j] == i) {
                idx = j;
                break;
            }
        }

        if (idx == -1) continue;

        int num_keys = GetNumKeys(i);
        for (int j = 0; j < num_keys; j++) {
            if (GetKey(i,j).m_extra == -2) {
                outliers.push_back(ImageKey(i,j));
            }
        }
    }

    int num_outliers = (int) outliers.size();
    fprintf(f, "%d\n", num_outliers);

    for (int i = 0; i < num_outliers; i++) {
        fprintf(f, "%d %d\n", outliers[i].first, outliers[i].second);
    }
#endif

    fclose(f);

    clock_t end = clock();

    printf("[DumpOutputFile] Wrote file in %0.3fs\n",
        (double) (end - start) / (double) CLOCKS_PER_SEC);
}