Factor graph optimization