Update guess function

[adithyashankar-97]

Hi @markisus

Thanks a lot for this amazing work. I was using the inside out tracker and i was wondering if you could explain this one function.

def update_guess(self, guess_idx, tags, force_update = False):
        tx_world_viewpoint = self.txs_world_viewpoint[guess_idx]
        error = self.errors[guess_idx]
        regularizer = self.regularizers[guess_idx]
        
        JtJ = np.zeros((6,6))
        rtJ = np.zeros((1,6))
        curr_error = 0

        for tag_id, corners in tags:
            tx_world_tag = self.txs_world_tag.get(tag_id, None)
            if tx_world_tag is None:
                continue

            corners = np.array(corners).reshape((8,1))
            tx_viewpoint_tag = SE3_inv(tx_world_viewpoint) @ tx_world_tag
            projected_corners, dcorners_dcamera, _ = project(self.camera_matrix, tx_viewpoint_tag, self.get_corners_mat(tag_id))

            residual = projected_corners - corners
            JtJ += dcorners_dcamera.T @ dcorners_dcamera
            rtJ += residual.T @ dcorners_dcamera
            curr_error += (residual.T @ residual)[0,0]

        if curr_error > error:
            regularizer *= 25
        else:
            regularizer *= 0.5

        improved = curr_error < error

        regularizer = min(regularizer, self.max_regularizer)
        regularizer = max(regularizer, 1e-3)

        if improved or force_update:
            update = np.linalg.solve(JtJ + regularizer * np.eye(6), -rtJ.T)
            tx_world_viewpoint = tx_world_viewpoint @ se3_exp(update)
            # tx_world_viewpoint = heuristic_flip_tx_world_cam(tx_world_viewpoint @ se3_exp(update))

        error = curr_error

        self.txs_world_viewpoint[guess_idx] = tx_world_viewpoint
        self.regularizers[guess_idx] = regularizer
        self.errors[guess_idx] = error

If possible can you please share a small blog or writeup on the total approach as well?

Thank you

[markisus]

Hi @adithyashankar-97

This is doing Levenberg Marquardt iterations. See this article https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm

In my code, J stands for Jacobian, and r for residual. This is probably very similar to what OpenCV's solvePnP does under the hood as well.